From 5f17f365e3458d5d7ccb23eb22fe8fcc3ba1c668 Mon Sep 17 00:00:00 2001 From: Marius Eggert Date: Tue, 27 Sep 2022 11:06:11 +0200 Subject: [PATCH] fixed local includes --- mission/controller/acs/ActuatorCmd.cpp | 6 +- mission/controller/acs/ActuatorCmd.h | 10 +- mission/controller/acs/Guidance.cpp | 10 +- mission/controller/acs/Guidance.h | 6 +- mission/controller/acs/Igrf13Model.cpp | 10 +- mission/controller/acs/Igrf13Model.h | 2 +- .../acs/MultiplicativeKalmanFilter.cpp | 12 +- .../acs/MultiplicativeKalmanFilter.h | 2 +- mission/controller/acs/Navigation.cpp | 11 +- mission/controller/acs/Navigation.h | 10 +- mission/controller/acs/OutputValues.cpp | 2 +- mission/controller/acs/SensorProcessing.cpp | 6 +- mission/controller/acs/SensorProcessing.h | 11 +- mission/controller/acs/SensorValues.cpp | 5 +- mission/controller/acs/SusConverter.cpp | 263 +++++++++--------- mission/controller/acs/SusConverter.h | 39 ++- mission/controller/acs/control/Detumble.cpp | 6 +- mission/controller/acs/control/Detumble.h | 11 +- mission/controller/acs/control/PtgCtrl.cpp | 12 +- mission/controller/acs/control/PtgCtrl.h | 10 +- mission/controller/acs/control/SafeCtrl.cpp | 11 +- mission/controller/acs/control/SafeCtrl.h | 11 +- 22 files changed, 235 insertions(+), 231 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 3b4ed71e..261277a0 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -6,14 +6,14 @@ */ -#include +#include "ActuatorCmd.h" +#include "util/MathOperations.h" +#include "util/CholeskyDecomposition.h" #include -#include #include #include #include #include -#include ActuatorCmd::ActuatorCmd(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 85ef1532..820f1f00 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -9,11 +9,11 @@ #define ACTUATORCMD_H_ -#include -#include -#include -#include -#include +#include "AcsParameters.h" +#include "SensorProcessing.h" +#include "MultiplicativeKalmanFilter.h" +#include "SensorValues.h" +#include "OutputValues.h" class ActuatorCmd{ public: diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 158ee9f0..a6a91a81 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -8,12 +8,12 @@ #include "Guidance.h" #include "string.h" +#include "util/MathOperations.h" +#include "util/CholeskyDecomposition.h" #include -#include -#include -#include -#include -#include +#include +#include +#include Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 736d8b9b..bf53d767 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -9,9 +9,9 @@ #define GUIDANCE_H_ -#include -#include -#include +#include "AcsParameters.h" +#include "SensorValues.h" +#include "OutputValues.h" #include diff --git a/mission/controller/acs/Igrf13Model.cpp b/mission/controller/acs/Igrf13Model.cpp index 91bfad15..75d3c9a1 100644 --- a/mission/controller/acs/Igrf13Model.cpp +++ b/mission/controller/acs/Igrf13Model.cpp @@ -6,15 +6,15 @@ */ #include "Igrf13Model.h" +#include "util/MathOperations.h" #include #include #include //#include -#include -#include -#include -#include -#include +#include +#include +#include +#include Igrf13Model::Igrf13Model(){ diff --git a/mission/controller/acs/Igrf13Model.h b/mission/controller/acs/Igrf13Model.h index 3ea2cb40..d89865ca 100644 --- a/mission/controller/acs/Igrf13Model.h +++ b/mission/controller/acs/Igrf13Model.h @@ -16,11 +16,11 @@ #ifndef IGRF13MODEL_H_ #define IGRF13MODEL_H_ -#include #include #include #include #include +#include // Output should be transformed to [T] instead of [nT] diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.cpp b/mission/controller/acs/MultiplicativeKalmanFilter.cpp index b2c8a963..5f7facd2 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.cpp +++ b/mission/controller/acs/MultiplicativeKalmanFilter.cpp @@ -5,14 +5,14 @@ * Author: rooob */ +#include "MultiplicativeKalmanFilter.h" +#include "util/CholeskyDecomposition.h" +#include "util/MathOperations.h" #include #include -#include -#include -#include -#include "MultiplicativeKalmanFilter.h" -#include -#include +#include +#include +#include /*Initialisation of values for parameters in constructor*/ MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_) : diff --git a/mission/controller/acs/MultiplicativeKalmanFilter.h b/mission/controller/acs/MultiplicativeKalmanFilter.h index f972ab5a..542db996 100644 --- a/mission/controller/acs/MultiplicativeKalmanFilter.h +++ b/mission/controller/acs/MultiplicativeKalmanFilter.h @@ -14,9 +14,9 @@ #ifndef MULTIPLICATIVEKALMANFILTER_H_ #define MULTIPLICATIVEKALMANFILTER_H_ +#include "config/classIds.h" #include //uint8_t #include /*purpose, timeval ?*/ -#include "acs/config/classIds.h" //#include <_timeval.h> #include "AcsParameters.h" diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 2cb70389..a2e3aa5d 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -6,12 +6,13 @@ */ #include "Navigation.h" +#include "util/MathOperations.h" +#include "util/CholeskyDecomposition.h" #include -#include -#include -#include -#include -#include +#include +#include +#include + Navigation::Navigation(AcsParameters *acsParameters_): multiplicativeKalmanFilter(acsParameters_){ acsParameters = *acsParameters_; diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h index 75bbade5..6691b8aa 100644 --- a/mission/controller/acs/Navigation.h +++ b/mission/controller/acs/Navigation.h @@ -9,11 +9,11 @@ #define NAVIGATION_H_ -#include -#include -#include -#include -#include +#include "AcsParameters.h" +#include "SensorProcessing.h" +#include "MultiplicativeKalmanFilter.h" +#include "SensorValues.h" +#include "OutputValues.h" class Navigation{ public: diff --git a/mission/controller/acs/OutputValues.cpp b/mission/controller/acs/OutputValues.cpp index 3deb4043..d035f621 100644 --- a/mission/controller/acs/OutputValues.cpp +++ b/mission/controller/acs/OutputValues.cpp @@ -4,7 +4,7 @@ * Created on: 30 Mar 2022 * Author: rooob */ -#include +#include "OutputValues.h" namespace ACS { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 7b89daf1..61f8092b 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -6,14 +6,14 @@ */ #include "SensorProcessing.h" +#include "Igrf13Model.h" +#include "util/MathOperations.h" +#include #include #include #include #include #include -#include -#include -#include using namespace Math; // Thought: Maybe separate file for transforming of sensor values diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h index 23abcc40..aa4dc3bd 100644 --- a/mission/controller/acs/SensorProcessing.h +++ b/mission/controller/acs/SensorProcessing.h @@ -5,13 +5,14 @@ #ifndef SENSORPROCESSING_H_ #define SENSORPROCESSING_H_ +#include "AcsParameters.h" +#include "SensorValues.h" +#include "OutputValues.h" +#include "config/classIds.h" #include //uint8_t #include /*purpose, timeval ?*/ -#include -#include -#include -#include -#include +#include + /*Planned: * - Fusion of Sensor Measurements - diff --git a/mission/controller/acs/SensorValues.cpp b/mission/controller/acs/SensorValues.cpp index 414c93ef..11720aeb 100644 --- a/mission/controller/acs/SensorValues.cpp +++ b/mission/controller/acs/SensorValues.cpp @@ -4,11 +4,10 @@ * Created on: 30 Mar 2022 * Author: rooob */ -#include - +#include "SensorValues.h" #include #include -#include +#include namespace ACS { diff --git a/mission/controller/acs/SusConverter.cpp b/mission/controller/acs/SusConverter.cpp index 151648ed..c2d65133 100644 --- a/mission/controller/acs/SusConverter.cpp +++ b/mission/controller/acs/SusConverter.cpp @@ -5,13 +5,15 @@ * Author: Timon Schwarz */ +#include "SusConverter.h" #include //for atan2 #include -#include #include +#include +#include void SunSensor::setSunSensorData() { - // Creates dummy sensordata, replace with SUS devicehandler / channel readout + // ToDo: exchange dummy values with DataPool susChannelValues[0] = {3913, 3912, 3799, 4056}; susChannelValues[1] = {3913, 3912, 3799, 4056}; susChannelValues[2] = {3913, 3912, 3799, 4056}; @@ -26,71 +28,73 @@ void SunSensor::setSunSensorData() { susChannelValues[11] = {3913, 3912, 3799, 4056}; } -void SunSensor::checkSunSensorData(uint8_t Sensornumber) { - uint16_t ChannelValueSum; +void SunSensor::checkSunSensorData(uint8_t susNumber) { + uint16_t channelValueSum; // Check individual channel values for (int k = 0; k < 4; k++) { // iteration above all photodiode quarters - if (susChannelValues[Sensornumber][k] <= ChannelValueCheckLow || - susChannelValues[Sensornumber][k] > ChannelValueCheckHigh) { // Channel values out of range for 12 bit SUS + if (susChannelValues[susNumber][k] <= channelValueCheckLow || + susChannelValues[susNumber][k] > channelValueCheckHigh) { // Channel values out of range for 12 bit SUS // channel measurement range? - ValidityNumber[Sensornumber] = false; // false --> Data not valid + validFlag[susNumber] = returnvalue::FAILED; /*printf( "The value of channel %i from sun sensor %i is not inside the borders of valid data with " "a value of %i \n", - k, Sensornumber, ChannelValue[k]);*/ - } else if (susChannelValues[Sensornumber][k] > - susChannelValues[Sensornumber][4]) { // Channel values higher than zero current threshold GNDREF? - ValidityNumber[Sensornumber] = false; + k, susNumber, ChannelValue[k]);*/ + } else if (susChannelValues[susNumber][k] > + susChannelValues[susNumber][4]) { // Channel values higher than zero current threshold GNDREF? + validFlag[susNumber] = returnvalue::FAILED; /*printf( "The value of channel %i from sun sensor %i is higher than the zero current threshold " "GNDREF\n", - k, Sensornumber);*/ + k, susNumber);*/ }; }; // check sum of all channel values to check if sun sensor is illuminated by the sun (sum is // smaller than a treshold --> sun sensor is not illuminated by the sun, but by the moon // reflection or earth albedo) - ChannelValueSum = - 4 * susChannelValues[Sensornumber][4] - (susChannelValues[Sensornumber][0] + - susChannelValues[Sensornumber][1] + susChannelValues[Sensornumber][2] + - susChannelValues[Sensornumber][3]); - if ((ChannelValueSum < ChannelValueSumHigh) && (ChannelValueSum > ChannelValueSumLow)) { - ValidityNumber[Sensornumber] = false; - //printf("Sun sensor %i is not illuminated by the sun\n", Sensornumber); + channelValueSum = + 4 * susChannelValues[susNumber][4] - (susChannelValues[susNumber][0] + + susChannelValues[susNumber][1] + susChannelValues[susNumber][2] + + susChannelValues[susNumber][3]); + if ((channelValueSum < channelValueSumHigh) && (channelValueSum > channelValueSumLow)) { + validFlag[susNumber] = returnvalue::FAILED; + //printf("Sun sensor %i is not illuminated by the sun\n", susNumber); }; } -void SunSensor::AngleCalculation(uint8_t Sensornumber) { - float xout, yout, s = 0.03; // s=[mm] - uint8_t d = 5, h = 1; // d=[mm] h=[mm] +void SunSensor::calcAngle(uint8_t susNumber) { + float xout, yout; + float s = 0.03; // s=[mm] gap between diodes + uint8_t d = 5; // d=[mm] edge length of the quadratic aperture + uint8_t h = 1; // h=[mm] distance between diodes and aperture int ch0, ch1, ch2, ch3; // Substract measurement values from GNDREF zero current threshold - ch0 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][0]; - ch1 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][1]; - ch2 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][2]; - ch3 = susChannelValues[Sensornumber][4] - susChannelValues[Sensornumber][3]; + ch0 = susChannelValues[susNumber][4] - susChannelValues[susNumber][0]; + ch1 = susChannelValues[susNumber][4] - susChannelValues[susNumber][1]; + ch2 = susChannelValues[susNumber][4] - susChannelValues[susNumber][2]; + ch3 = susChannelValues[susNumber][4] - susChannelValues[susNumber][3]; // Calculation of x and y xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm] // Calculation of the angles - AlphaBetaRaw[Sensornumber][0] = atan2(xout, h) * (180 / M_PI); //[°] - AlphaBetaRaw[Sensornumber][1] = atan2(yout, h) * (180 / M_PI); //[°] + alphaBetaRaw[susNumber][0] = atan2(xout, h) * (180 / M_PI); //[°] + alphaBetaRaw[susNumber][1] = atan2(yout, h) * (180 / M_PI); //[°] } -void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { - switch (Sensornumber) { // search for the correct calibration coefficients for each SUS +void SunSensor::setCalibrationCoefficients(uint8_t susNumber) { + switch (susNumber) { // search for the correct calibration coefficients for each SUS case 0: for (uint8_t row = 0; row < 9; row++) { // save the correct coefficients in the right SUS class for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus0coeffBeta[row][column]; } } break; @@ -98,8 +102,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 1: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus1coeffBeta[row][column]; } } break; @@ -107,8 +111,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 2: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus2coeffBeta[row][column]; } } break; @@ -116,8 +120,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 3: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus3coeffBeta[row][column]; } } break; @@ -125,8 +129,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 4: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus4coeffBeta[row][column]; } } break; @@ -134,8 +138,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 5: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus5coeffBeta[row][column]; } } break; @@ -143,8 +147,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 6: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus6coeffBeta[row][column]; } } break; @@ -152,8 +156,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 7: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus7coeffBeta[row][column]; } } break; @@ -161,8 +165,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 8: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus8coeffBeta[row][column]; } } break; @@ -170,8 +174,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 9: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus9coeffBeta[row][column]; } } break; @@ -179,8 +183,8 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 10: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus10coeffBeta[row][column]; } } break; @@ -188,20 +192,20 @@ void SunSensor::setCalibrationCoefficients(uint8_t Sensornumber) { case 11: for (uint8_t row = 0; row < 9; row++) { for (uint8_t column = 0; column < 10; column++) { - CoeffAlpha[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; - CoeffBeta[Sensornumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; + coeffAlpha[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffAlpha[row][column]; + coeffBeta[susNumber][row][column] = acsParameters.susHandlingParameters.sus11coeffBeta[row][column]; } } break; } } -void SunSensor::Calibration(uint8_t Sensornumber) { +void SunSensor::Calibration(uint8_t susNumber) { float alpha_m, beta_m, alpha_calibrated, beta_calibrated, k, l; uint8_t index; - alpha_m = AlphaBetaRaw[Sensornumber][0]; //[°] - beta_m = AlphaBetaRaw[Sensornumber][1]; //[°] + alpha_m = alphaBetaRaw[susNumber][0]; //[°] + beta_m = alphaBetaRaw[susNumber][1]; //[°] // while loop iterates above all calibration cells to use the different calibration functions in // each cell @@ -212,47 +216,47 @@ void SunSensor::Calibration(uint8_t Sensornumber) { while (l < 3) { l = l + 1; // if-condition to check in which cell the data point has to be - if ((alpha_m > ((CompleteCellWidth * ((k - 1) / 3)) - HalfCellWidth) && - alpha_m < ((CompleteCellWidth * (k / 3)) - HalfCellWidth)) && - (beta_m > ((CompleteCellWidth * ((l - 1) / 3)) - HalfCellWidth) && - beta_m < ((CompleteCellWidth * (l / 3)) - HalfCellWidth))) { + if ((alpha_m > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) && + alpha_m < ((completeCellWidth * (k / 3)) - halfCellWidth)) && + (beta_m > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) && + beta_m < ((completeCellWidth * (l / 3)) - halfCellWidth))) { index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell // -> first cell has number 0 - AlphaBetaCalibrated[Sensornumber][0] = - CoeffAlpha[Sensornumber][index][0] + CoeffAlpha[Sensornumber][index][1] * alpha_m + CoeffAlpha[Sensornumber][index][2] * beta_m + - CoeffAlpha[Sensornumber][index][3] * alpha_m * alpha_m + CoeffAlpha[Sensornumber][index][4] * alpha_m * beta_m + - CoeffAlpha[Sensornumber][index][5] * beta_m * beta_m + - CoeffAlpha[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + - CoeffAlpha[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + - CoeffAlpha[Sensornumber][index][8] * alpha_m * beta_m * beta_m + - CoeffAlpha[Sensornumber][index][9] * beta_m * beta_m * beta_m; //[°] - AlphaBetaCalibrated[Sensornumber][1] = - CoeffBeta[Sensornumber][index][0] + CoeffBeta[Sensornumber][index][1] * alpha_m + - CoeffBeta[Sensornumber][index][2] * beta_m + CoeffBeta[Sensornumber][index][3] * alpha_m * alpha_m + - CoeffBeta[Sensornumber][index][4] * alpha_m * beta_m + - CoeffBeta[Sensornumber][index][5] * beta_m * beta_m + - CoeffBeta[Sensornumber][index][6] * alpha_m * alpha_m * alpha_m + - CoeffBeta[Sensornumber][index][7] * alpha_m * alpha_m * beta_m + - CoeffBeta[Sensornumber][index][8] * alpha_m * beta_m * beta_m + - CoeffBeta[Sensornumber][index][9] * beta_m * beta_m * beta_m; //[°] + alphaBetaCalibrated[susNumber][0] = + coeffAlpha[susNumber][index][0] + coeffAlpha[susNumber][index][1] * alpha_m + coeffAlpha[susNumber][index][2] * beta_m + + coeffAlpha[susNumber][index][3] * alpha_m * alpha_m + coeffAlpha[susNumber][index][4] * alpha_m * beta_m + + coeffAlpha[susNumber][index][5] * beta_m * beta_m + + coeffAlpha[susNumber][index][6] * alpha_m * alpha_m * alpha_m + + coeffAlpha[susNumber][index][7] * alpha_m * alpha_m * beta_m + + coeffAlpha[susNumber][index][8] * alpha_m * beta_m * beta_m + + coeffAlpha[susNumber][index][9] * beta_m * beta_m * beta_m; //[°] + alphaBetaCalibrated[susNumber][1] = + coeffBeta[susNumber][index][0] + coeffBeta[susNumber][index][1] * alpha_m + + coeffBeta[susNumber][index][2] * beta_m + coeffBeta[susNumber][index][3] * alpha_m * alpha_m + + coeffBeta[susNumber][index][4] * alpha_m * beta_m + + coeffBeta[susNumber][index][5] * beta_m * beta_m + + coeffBeta[susNumber][index][6] * alpha_m * alpha_m * alpha_m + + coeffBeta[susNumber][index][7] * alpha_m * alpha_m * beta_m + + coeffBeta[susNumber][index][8] * alpha_m * beta_m * beta_m + + coeffBeta[susNumber][index][9] * beta_m * beta_m * beta_m; //[°] } } } } -void SunSensor::CalculateSunVector(uint8_t Sensornumber) { +void SunSensor::CalculateSunVector(uint8_t susNumber) { float alpha, beta; - alpha = AlphaBetaCalibrated[Sensornumber][0]; //[°] - beta = AlphaBetaCalibrated[Sensornumber][1]; //[°] + alpha = alphaBetaCalibrated[susNumber][0]; //[°] + beta = alphaBetaCalibrated[susNumber][1]; //[°] // Calculate the normalized Sun Vector - SunVectorBodyFrame[Sensornumber][0] = + sunVectorBodyFrame[susNumber][0] = (tan(alpha * (M_PI / 180)) / (sqrt((powf(tan(alpha * (M_PI / 180)), 2)) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - SunVectorBodyFrame[Sensornumber][1] = + sunVectorBodyFrame[susNumber][1] = (tan(beta * (M_PI / 180)) / (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf(tan((beta * (M_PI / 180))), 2) + (1)))); - SunVectorBodyFrame[Sensornumber][2] = + sunVectorBodyFrame[susNumber][2] = (-1 / (sqrt(powf((tan(alpha * (M_PI / 180))), 2) + powf((tan(beta * (M_PI / 180))), 2) + (1)))); } @@ -262,79 +266,79 @@ float* SunSensor::getSunVectorBodyFrame() { float* SunVectorBodyFrameReturn = 0; SunVectorBodyFrameReturn = new float[3]; - SunVectorBodyFrameReturn[0] = SunVectorBodyFrame[0]; - SunVectorBodyFrameReturn[1] = SunVectorBodyFrame[1]; - SunVectorBodyFrameReturn[2] = SunVectorBodyFrame[2]; + SunVectorBodyFrameReturn[0] = sunVectorBodyFrame[0]; + SunVectorBodyFrameReturn[1] = sunVectorBodyFrame[1]; + SunVectorBodyFrameReturn[2] = sunVectorBodyFrame[2]; return SunVectorBodyFrameReturn; } -bool SunSensor::getValidityNumber(uint8_t Sensornumber) { - return ValidityNumber[Sensornumber]; +bool SunSensor::getValidFlag(uint8_t susNumber) { + return validFlag[susNumber]; } float* SunSensor::TransferSunVector() { - float* SunVectorEIVE = 0; - SunVectorEIVE = new float[3]; + float* sunVectorEIVE = 0; + sunVectorEIVE = new float[3]; - uint8_t counter = 0; - int8_t BasisMatrixUse[3][3]; - float SunVectorMatrixEIVE[3][12] = {0}, sum; - float SunVectorMatrixBodyFrame[3][12]; + uint8_t susAvail = 12; + int8_t basisMatrixUse[3][3]; + float sunVectorMatrixEIVE[3][12] = {0}; + float sunVectorMatrixBodyFrame[3][12]; - for (uint8_t Sensornumber = 0; Sensornumber < 12; - Sensornumber++) { // save the sun vector of each SUS in their body frame into an array for + 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[Sensornumber]; - SunVectorMatrixBodyFrame[0][Sensornumber] = SunVectorBodyFrame[0]; - SunVectorMatrixBodyFrame[1][Sensornumber] = SunVectorBodyFrame[1]; - SunVectorMatrixBodyFrame[2][Sensornumber] = SunVectorBodyFrame[2]; + float* SunVectorBodyFrame = SunVectorBodyFrame[susNumber]; + sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0]; + sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1]; + sunVectorMatrixBodyFrame[2][susNumber] = SunVectorBodyFrame[2]; } - for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { - if (getValidityNumber(Sensornumber) == false) { - counter = counter + 1; + for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { + if (getValidFlag(susNumber) == returnvalue::FAILED) { + susAvail -= 1; } // if the SUS data is not valid -> for (uint8_t c1 = 0; c1 < 3; c1++) { for (uint8_t c2 = 0; c2 < 3; c2++) { - switch (Sensornumber) { // find right basis matrix for each SUS + switch (susNumber) { case 0: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2]; break; case 1: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2]; break; case 2: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2]; break; case 3: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2]; break; case 4: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2]; break; case 5: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2]; break; case 6: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2]; break; case 7: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2]; break; case 8: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2]; break; case 9: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2]; break; case 10: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2]; break; case 11: - BasisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; + basisMatrixUse[c1][c2] = acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2]; break; } } @@ -344,34 +348,31 @@ float* SunSensor::TransferSunVector() { for (uint8_t p = 0; p < 3; p++) { for (uint8_t q = 0; q < 3; q++) { // normal matrix multiplication - SunVectorMatrixEIVE[p][Sensornumber] += - (BasisMatrixUse[p][q] * SunVectorMatrixBodyFrame[q][Sensornumber]); + sunVectorMatrixEIVE[p][susNumber] += + (basisMatrixUse[p][q] * sunVectorMatrixBodyFrame[q][susNumber]); } } } - if (counter < 12) { // Calculate one sun vector out of all sun vectors from the different SUS + if (susAvail > 0) { // Calculate one sun vector out of all sun vectors from the different SUS for (uint8_t i = 0; i < 3; i++) { - sum = 0; - for (uint8_t Sensornumber = 0; Sensornumber < 12; Sensornumber++) { - if (getValidityNumber(Sensornumber)){ - sum += SunVectorMatrixEIVE[i][Sensornumber]; - //printf("%f\n", SunVectorMatrixEIVE[i][Sensornumber]); + float sum = 0; + for (uint8_t susNumber = 0; susNumber < 12; susNumber++) { + if (getValidFlag(susNumber) == returnvalue::OK){ + sum += sunVectorMatrixEIVE[i][susNumber]; + //printf("%f\n", SunVectorMatrixEIVE[i][susNumber]); } } // ToDo: decide on length on sun vector - SunVectorEIVE[i] = - sum/* / (12 - counter)*/; // FLAG Ergebnis ist falsch, kann an einem Fehler im Programm - // liegen, vermutlich aber an den falschen ChannelValues da die - // transformierten Sonnenvektoren jedes SUS plausibel sind + sunVectorEIVE[i] = sum; } - VectorOperations::normalize(SunVectorEIVE, SunVectorEIVE, 3); + VectorOperations::normalize(sunVectorEIVE, sunVectorEIVE, 3); } else { // No sus is valid throw std::invalid_argument("No sun sensor is valid"); // throw error } - return SunVectorEIVE; + return sunVectorEIVE; } diff --git a/mission/controller/acs/SusConverter.h b/mission/controller/acs/SusConverter.h index 4012bb5a..3a5662ca 100644 --- a/mission/controller/acs/SusConverter.h +++ b/mission/controller/acs/SusConverter.h @@ -8,49 +8,48 @@ #ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ #define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ - +#include "AcsParameters.h" #include -#include class SunSensor { public: SunSensor() {} void setSunSensorData(); - void checkSunSensorData(uint8_t Sensornumber); - void AngleCalculation(uint8_t Sensornumber); - void setCalibrationCoefficients(uint8_t Sensornumber); - void Calibration(uint8_t Sensornumber); - void CalculateSunVector(uint8_t Sensornumber); + void checkSunSensorData(uint8_t susNumber); + void calcAngle(uint8_t susNumber); + void setCalibrationCoefficients(uint8_t susNumber); + void Calibration(uint8_t susNumber); + void CalculateSunVector(uint8_t susNumber); - bool getValidityNumber(uint8_t Sensornumber); + bool getValidFlag(uint8_t susNumber); float* getSunVectorBodyFrame(); float* TransferSunVector(); private: uint16_t susChannelValues[12][4]; //[Bit] - float AlphaBetaRaw[12][2]; //[°] - float AlphaBetaCalibrated[12][2]; //[°] - float SunVectorBodyFrame[12][3]; //[-] + float alphaBetaRaw[12][2]; //[°] + float alphaBetaCalibrated[12][2]; //[°] + float sunVectorBodyFrame[12][3]; //[-] - bool ValidityNumber[12] = true; + bool validFlag[12] = returnvalue::OK; - uint16_t ChannelValueCheckHigh = + uint16_t channelValueCheckHigh = 4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check - uint8_t ChannelValueCheckLow = + uint8_t channelValueCheckLow = 0; //[Bit]low borderline for the channel values of one sun sensor for validity Check - uint16_t ChannelValueSumHigh = + uint16_t channelValueSumHigh = 100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by // the reflection of sunlight from the moon/earth - uint8_t ChannelValueSumLow = + uint8_t channelValueSumLow = 0; //[Bit]low borderline for check if the sun sensor is illuminated // by the sun or by the reflection of sunlight from the moon/earth - uint8_t CompleteCellWidth = 140, - HalfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in + uint8_t completeCellWidth = 140, + halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in // which cell a data point should be - float CoeffAlpha[12][9][10]; - float CoeffBeta[12][9][10]; + float coeffAlpha[12][9][10]; + float coeffBeta[12][9][10]; AcsParameters acsParameters; }; diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 8a15aa8a..67903c7c 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,13 +7,13 @@ */ -#include +#include "Detumble.h" +#include "../util/MathOperations.h" +#include #include #include -#include #include #include -#include #include diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 618be6c6..835c32b0 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -8,14 +8,15 @@ #ifndef ACS_CONTROL_DETUMBLE_H_ #define ACS_CONTROL_DETUMBLE_H_ +#include "../SensorValues.h" +#include "../OutputValues.h" +#include "../AcsParameters.h" +#include "../config/classIds.h" #include #include -#include -#include -#include -#include -#include #include +#include + class Detumble{ diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index eaffd909..999df912 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -8,12 +8,12 @@ #include "PtgCtrl.h" -#include -#include -#include -#include -#include -#include +#include "../util/MathOperations.h" +#include +#include +#include +#include +#include #include PtgCtrl::PtgCtrl(AcsParameters *acsParameters_){ diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fe5d0e96..87a66f2d 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -14,12 +14,12 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ +#include "../SensorValues.h" +#include "../OutputValues.h" +#include "../AcsParameters.h" +#include "../config/classIds.h" #include #include -#include -#include -#include -#include #include class PtgCtrl{ @@ -53,7 +53,7 @@ private: AcsParameters::PointingModeControllerParameters* pointingModeControllerParameters; AcsParameters::RwHandlingParameters* rwHandlingParameters; AcsParameters::InertiaEIVE* inertiaEIVE; - AcsParameters::RWMatrices* rwMatrices; + AcsParameters::RwMatrices* rwMatrices; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 55772ae4..ff619f35 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -6,12 +6,13 @@ */ #include "SafeCtrl.h" -#include -#include -#include -#include -#include +#include "../util/MathOperations.h" #include +#include +#include +#include +#include + SafeCtrl::SafeCtrl(AcsParameters *acsParameters_){ loadAcsParameters(acsParameters_); diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 36807bcc..c6d47324 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -8,14 +8,15 @@ #ifndef SAFECTRL_H_ #define SAFECTRL_H_ +#include "../SensorValues.h" +#include "../OutputValues.h" +#include "../AcsParameters.h" +#include "../config/classIds.h" #include #include -#include -#include -#include -#include "acs/config/classIds.h" -#include #include +#include + class SafeCtrl{