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

First Version of ACS Controller #329

Merged
muellerr merged 106 commits from acs-ctrl-v1 into develop 2022-12-02 16:21:58 +01:00
Conversation 0 Commits 106 Files Changed 50 +4 -113
1 changed files with 4 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit bbf4be1cfe - Show all commits

117
mission/controller/acs/SusConverter.cpp
View File

@ -67,16 +67,17 @@ void SusConverter::calcAngle(lp_vec_t<uint16_t, 6> susChannel) {
}
void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
uint8_t index, k, l;
uint8_t index;
float k, l;
// while loop iterates above all calibration cells to use the different calibration functions in
// each cell
k = 0;
while (k < 3) {
k = k + 1;
k++;
l = 0;
while (l < 3) {
l = l + 1;
l++;
// if-condition to check in which cell the data point has to be
if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) &&
alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) &&
@ -131,113 +132,3 @@ float* SusConverter::getSunVectorSensorFrame(lp_vec_t<uint16_t, 6> susChannel,
return calculateSunVector();
}
bool SusConverter::getValidFlag(uint8_t susNumber) { return validFlag[susNumber]; }
float* SusConverter::TransferSunVector() {
float* sunVectorEIVE = 0;
sunVectorEIVE = new float[3];
uint8_t susAvail = 12;
int8_t basisMatrixUse[3][3];
float sunVectorMatrixEIVE[3][12] = {0};
float sunVectorMatrixBodyFrame[3][12];
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];
sunVectorMatrixBodyFrame[0][susNumber] = SunVectorBodyFrame[0];
sunVectorMatrixBodyFrame[1][susNumber] = SunVectorBodyFrame[1];
sunVectorMatrixBodyFrame[2][susNumber] = SunVectorBodyFrame[2];
}
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 (susNumber) {
case 0:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus0orientationMatrix[c1][c2];
break;
case 1:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus1orientationMatrix[c1][c2];
break;
case 2:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus2orientationMatrix[c1][c2];
break;
case 3:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus3orientationMatrix[c1][c2];
break;
case 4:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus4orientationMatrix[c1][c2];
break;
case 5:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus5orientationMatrix[c1][c2];
break;
case 6:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus6orientationMatrix[c1][c2];
break;
case 7:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus7orientationMatrix[c1][c2];
break;
case 8:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus8orientationMatrix[c1][c2];
break;
case 9:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus9orientationMatrix[c1][c2];
break;
case 10:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus10orientationMatrix[c1][c2];
break;
case 11:
basisMatrixUse[c1][c2] =
acsParameters.susHandlingParameters.sus11orientationMatrix[c1][c2];
break;
}
}
}
// matrix multiplication for transition in EIVE coordinatesystem
for (uint8_t p = 0; p < 3; p++) {
for (uint8_t q = 0; q < 3; q++) {
// normal matrix multiplication
sunVectorMatrixEIVE[p][susNumber] +=
(basisMatrixUse[p][q] * sunVectorMatrixBodyFrame[q][susNumber]);
}
}
}
if (susAvail > 0) { // Calculate one sun vector out of all sun vectors from the different SUS
for (uint8_t i = 0; i < 3; i++) {
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;
}
VectorOperations<float>::normalize(sunVectorEIVE, sunVectorEIVE, 3);
} else {
// No sus is valid
throw std::invalid_argument("No sun sensor is valid"); // throw error
}
return sunVectorEIVE;
}