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

switched sensor input from AcsController to SensorValues
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
Details

Browse Source
This commit is contained in:
Marius Eggert
2022-10-10 16:02:57 +02:00
parent 82e9c6d092
commit 076e67dd0b
6 changed files with 241 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -14,30 +14,26 @@
#include <iostream>
bool SusConverter::checkSunSensorData(lp_vec_t<uint16_t, 6> susChannel) {
if (susChannel.value[0] <= susChannelValueCheckLow ||
susChannel.value[0] > susChannelValueCheckHigh ||
susChannel.value[0] > susChannel.value[GNDREF]) {
bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh ||
susChannel[0] > susChannel[GNDREF]) {
return false;
}
if (susChannel.value[1] <= susChannelValueCheckLow ||
susChannel.value[1] > susChannelValueCheckHigh ||
susChannel.value[1] > susChannel.value[GNDREF]) {
if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh ||
susChannel[1] > susChannel[GNDREF]) {
return false;
};
if (susChannel.value[2] <= susChannelValueCheckLow ||
susChannel.value[2] > susChannelValueCheckHigh ||
susChannel.value[2] > susChannel.value[GNDREF]) {
if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh ||
susChannel[2] > susChannel[GNDREF]) {
return false;
};
if (susChannel.value[3] <= susChannelValueCheckLow ||
susChannel.value[3] > susChannelValueCheckHigh ||
susChannel.value[3] > susChannel.value[GNDREF]) {
if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh ||
susChannel[3] > susChannel[GNDREF]) {
return false;
};
susChannelValueSum = 4 * susChannel.value[GNDREF] - (susChannel.value[0] + susChannel.value[1] +
susChannel.value[2] + susChannel.value[3]);
susChannelValueSum =
4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]);
if ((susChannelValueSum < susChannelValueSumHigh) &&
(susChannelValueSum > susChannelValueSumLow)) {
return false;
@ -45,17 +41,17 @@ bool SusConverter::checkSunSensorData(lp_vec_t<uint16_t, 6> susChannel) {
return true;
}
void SusConverter::calcAngle(lp_vec_t<uint16_t, 6> susChannel) {
void SusConverter::calcAngle(const uint16_t susChannel[6]) {
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 = susChannel.value[GNDREF] - susChannel.value[0];
ch1 = susChannel.value[GNDREF] - susChannel.value[1];
ch2 = susChannel.value[GNDREF] - susChannel.value[2];
ch3 = susChannel.value[GNDREF] - susChannel.value[3];
ch0 = susChannel[GNDREF] - susChannel[0];
ch1 = susChannel[GNDREF] - susChannel[1];
ch2 = susChannel[GNDREF] - susChannel[2];
ch3 = susChannel[GNDREF] - susChannel[3];
// Calculation of x and y
xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm]
@ -124,11 +120,10 @@ float* SusConverter::calculateSunVector() {
return sunVectorBodyFrame;
}
float* SusConverter::getSunVectorSensorFrame(lp_vec_t<uint16_t, 6> susChannel,
float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6],
const float coeffAlpha[9][10],
const float coeffBeta[9][10]) {
calcAngle(susChannel);
calibration(coeffAlpha, coeffBeta);
return calculateSunVector();
}