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

onboard handling of GYR values changed from deg/s to rad/s

Browse Source
This commit is contained in:
Marius Eggert 2023-02-07 10:20:36 +01:00
parent 6567fd1023
commit 5d0a1858cc
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
mission/controller/acs/AcsParameters.h
View File

@ -915,8 +915,8 @@ class AcsParameters : public HasParametersIF {
struct DetumbleParameter { struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2; double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 0.4; double omegaDetumbleEnd = 0.4 * M_PI / 180;
double gainD = pow(10.0, -3.3); double gainD = pow(10.0, -3.3);
} detumbleParameter; } detumbleParameter;
}; };

4
mission/controller/acs/SensorProcessing.cpp
View File

@ -477,6 +477,7 @@ void SensorProcessing::processGyr(
const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue}; const double gyr0Value[3] = {gyr0axXvalue, gyr0axYvalue, gyr0axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value, MatrixOperations<double>::multiply(gyrParameters->gyr0orientationMatrix[0], gyr0Value,
gyr0ValueBody, 3, 3, 1); gyr0ValueBody, 3, 3, 1);
VectorOperations<double>::mulScalar(gyr0ValueBody, M_PI / 180, gyr0ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr0ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@ -486,6 +487,7 @@ void SensorProcessing::processGyr(
const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue}; const double gyr1Value[3] = {gyr1axXvalue, gyr1axYvalue, gyr1axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value, MatrixOperations<double>::multiply(gyrParameters->gyr1orientationMatrix[0], gyr1Value,
gyr1ValueBody, 3, 3, 1); gyr1ValueBody, 3, 3, 1);
VectorOperations<double>::mulScalar(gyr1ValueBody, M_PI / 180, gyr1ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr1ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];
@ -495,6 +497,7 @@ void SensorProcessing::processGyr(
const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue}; const double gyr2Value[3] = {gyr2axXvalue, gyr2axYvalue, gyr2axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value, MatrixOperations<double>::multiply(gyrParameters->gyr2orientationMatrix[0], gyr2Value,
gyr2ValueBody, 3, 3, 1); gyr2ValueBody, 3, 3, 1);
VectorOperations<double>::mulScalar(gyr2ValueBody, M_PI / 180, gyr2ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i]; sensorFusionNumerator[i] += gyr2ValueBody[i] / gyrParameters->gyr02variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr02variance[i];
@ -504,6 +507,7 @@ void SensorProcessing::processGyr(
const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue}; const double gyr3Value[3] = {gyr3axXvalue, gyr3axYvalue, gyr3axZvalue};
MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value, MatrixOperations<double>::multiply(gyrParameters->gyr3orientationMatrix[0], gyr3Value,
gyr3ValueBody, 3, 3, 1); gyr3ValueBody, 3, 3, 1);
VectorOperations<double>::mulScalar(gyr3ValueBody, M_PI / 180, gyr3ValueBody, 3);
for (uint8_t i = 0; i < 3; i++) { for (uint8_t i = 0; i < 3; i++) {
sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i]; sensorFusionNumerator[i] += gyr3ValueBody[i] / gyrParameters->gyr13variance[i];
sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i]; sensorFusionDenominator[i] += 1 / gyrParameters->gyr13variance[i];