eive-obsw/mission/controller/acs/Guidance.cpp

333 lines
16 KiB
C++
Raw Normal View History

2022-09-19 15:17:39 +02:00
#include "Guidance.h"
2022-10-12 15:06:24 +02:00
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
2022-09-19 15:17:39 +02:00
2022-10-12 15:06:24 +02:00
Guidance::~Guidance() {}
2022-09-19 15:17:39 +02:00
2024-02-05 09:39:35 +01:00
void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
const double sunDirI[3], const double posSatF[4],
double targetQuat[4], double targetSatRotRate[3]) {
// positive z-Axis of EIVE in direction of sun
2024-02-12 11:09:14 +01:00
double zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirI, zAxisIX, 3);
2022-10-12 15:06:24 +02:00
// determine helper vector to point x-Axis and therefore the STR away from Earth
double helperXI[3] = {0, 0, 0}, posSatI[3] = {0, 0, 0};
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
VectorOperations<double>::normalize(posSatI, helperXI, 3);
2022-10-12 15:06:24 +02:00
// construct y-axis from helper vector and z-axis
2024-02-12 11:09:14 +01:00
double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisIX, helperXI, yAxisIX);
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
2022-10-12 15:06:24 +02:00
// x-axis completes RHS
2024-02-12 11:09:14 +01:00
double xAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxisIX, zAxisIX, xAxisIX);
VectorOperations<double>::normalize(xAxisIX, xAxisIX, 3);
2024-02-02 14:32:12 +01:00
// join transformation matrix
2024-02-12 11:09:14 +01:00
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
QuaternionOperations::fromDcm(dcmIX, targetQuat);
2024-02-02 14:32:12 +01:00
// calculate of reference rotation rate
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
2022-09-19 15:17:39 +02:00
}
2024-02-13 16:59:50 +01:00
void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
const double posSatF[3], const double velSatF[3],
double targetQuat[4], double targetSatRotRate[3]) {
2022-12-13 11:51:03 +01:00
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
2023-02-20 15:59:01 +01:00
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
2024-02-02 14:32:12 +01:00
double targetF[3] = {0, 0, 0};
2024-02-12 14:43:34 +01:00
CoordinateTransformations::cartesianFromLatLongAlt(
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
2024-02-02 14:32:12 +01:00
acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
double targetDirF[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetF, posSatF, targetDirF, 3);
2022-12-13 11:51:03 +01:00
// target direction in the ECI frame
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
2024-02-02 14:32:12 +01:00
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
CoordinateTransformations::positionEcfToEci(targetF, targetI, &timeAbsolute);
VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
2022-12-13 11:51:03 +01:00
// x-axis aligned with target direction
// this aligns with the camera, E- and S-band antennas
2024-02-12 11:09:14 +01:00
double xAxisIX[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirI, xAxisIX, 3);
2022-12-13 11:51:03 +01:00
// transform velocity into inertial frame
2024-02-02 14:32:12 +01:00
double velSatI[3] = {0, 0, 0};
CoordinateTransformations::velocityEcfToEci(velSatF, posSatF, velSatI, &timeAbsolute);
2022-12-13 11:51:03 +01:00
// orbital normal vector of target and velocity vector
double orbitalNormalI[3] = {0, 0, 0};
2024-02-02 14:32:12 +01:00
VectorOperations<double>::cross(posSatI, velSatI, orbitalNormalI);
VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
2022-12-13 11:51:03 +01:00
// y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
// resolution
2024-02-12 11:09:14 +01:00
double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalI, xAxisIX, yAxisIX);
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
2022-12-13 11:51:03 +01:00
// z-axis completes RHS
2024-02-12 11:09:14 +01:00
double zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(xAxisIX, yAxisIX, zAxisIX);
2022-12-13 11:51:03 +01:00
// join transformation matrix
2024-02-12 11:09:14 +01:00
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
2023-02-20 16:14:40 +01:00
QuaternionOperations::fromDcm(dcmIX, targetQuat);
2022-12-13 11:51:03 +01:00
2024-02-02 14:32:12 +01:00
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
}
2024-02-13 15:58:44 +01:00
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
const double posSatF[3], const double sunDirI[3],
double targetQuat[4], double targetSatRotRate[3]) {
2022-12-13 11:51:03 +01:00
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
2022-12-13 11:51:03 +01:00
//-------------------------------------------------------------------------------------
2023-02-20 15:59:01 +01:00
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
2024-02-02 14:32:12 +01:00
double posGroundStationF[3] = {0, 0, 0};
2024-02-12 14:43:34 +01:00
CoordinateTransformations::cartesianFromLatLongAlt(
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
2024-02-02 14:32:12 +01:00
acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
// target direction in the ECI frame
2024-02-02 14:32:12 +01:00
double posSatI[3] = {0, 0, 0}, posGroundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
CoordinateTransformations::positionEcfToEci(posSatF, posSatI, &timeAbsolute);
CoordinateTransformations::positionEcfToEci(posGroundStationI, posGroundStationI, &timeAbsolute);
VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
2023-02-20 17:39:03 +01:00
// negative x-axis aligned with target direction
2023-02-20 15:59:01 +01:00
// this aligns with the camera, E- and S-band antennas
2024-02-12 11:09:14 +01:00
double xAxisIX[3] = {0, 0, 0};
VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
2024-02-13 15:58:44 +01:00
// get earth vector in ECI
2024-02-13 16:59:50 +01:00
double earthDirI[3] = {0, 0, 0};
VectorOperations<double>::normalize(posSatI, earthDirI, 3);
VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
2024-02-13 15:58:44 +01:00
// sun avoidance calculations
double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0};
2024-02-13 16:59:50 +01:00
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
sunPerpendicularX, 3);
VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
2024-02-13 15:58:44 +01:00
VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
double sunWeight = 0, strVecSun[3] = {0, 0, 0}, strVecSunX[3] = {0, 0, 0},
strVecSunZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
strVecSunX, 3);
VectorOperations<double>::mulScalar(sunFloorYZ, acsParameters->strParameters.boresightAxis[2],
strVecSunZ, 3);
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
2024-02-13 16:59:50 +01:00
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
2024-02-13 15:58:44 +01:00
// earth avoidance calculations
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0};
2024-02-13 16:59:50 +01:00
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
earthPerpendicularX, 3);
VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
2024-02-13 15:58:44 +01:00
VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
double earthWeight = 0, strVecEarth[3] = {0, 0, 0}, strVecEarthX[3] = {0, 0, 0},
strVecEarthZ[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, acsParameters->strParameters.boresightAxis[0],
strVecEarthX, 3);
VectorOperations<double>::mulScalar(earthFloorYZ, acsParameters->strParameters.boresightAxis[2],
strVecEarthZ, 3);
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
2024-02-13 16:59:50 +01:00
earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirI);
2024-02-13 15:58:44 +01:00
if ((sunWeight == 0.0) and (earthWeight == 0.0)) {
// if this actually ever happens i will eat a broom
sunWeight = 0.5;
earthWeight = 0.5;
}
// normalize weights for convenience
double normFactor = 1. / (std::fabs(sunWeight) + std::fabs(earthWeight));
2024-02-13 15:58:44 +01:00
sunWeight *= normFactor;
earthWeight *= normFactor;
// calculate z-axis for str blinding avoidance
double zAxisSun[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0}, zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(sunFloorYZ, sunWeight, zAxisSun, 3);
VectorOperations<double>::mulScalar(earthFloorYZ, earthWeight, zAxisEarth, 3);
VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
2024-02-12 11:09:14 +01:00
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
2024-02-13 15:58:44 +01:00
// calculate y-axis
double yAxisIX[3] = {0, 0, 0};
2024-02-12 11:09:14 +01:00
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
2023-02-20 15:59:01 +01:00
// join transformation matrix
2024-02-12 11:09:14 +01:00
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
QuaternionOperations::fromDcm(dcmIX, targetQuat);
2024-02-02 14:32:12 +01:00
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
}
2024-02-13 16:59:50 +01:00
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
const double posSatE[3], const double velSatE[3],
double targetQuat[4], double refSatRate[3]) {
2022-12-13 11:51:03 +01:00
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
2023-02-20 16:14:40 +01:00
// satellite position in inertial reference frame
double posSatI[3] = {0, 0, 0};
2024-02-02 14:32:12 +01:00
CoordinateTransformations::positionEcfToEci(posSatE, posSatI, &timeAbsolute);
2022-12-13 11:51:03 +01:00
2023-02-20 16:14:40 +01:00
// negative x-axis aligned with position vector
// this aligns with the camera, E- and S-band antennas
2024-02-12 11:09:14 +01:00
double xAxisIX[3] = {0, 0, 0};
VectorOperations<double>::normalize(posSatI, xAxisIX, 3);
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
2022-12-13 11:51:03 +01:00
2023-02-20 16:14:40 +01:00
// make z-Axis parallel to major part of camera resolution
2024-02-12 11:09:14 +01:00
double zAxisIX[3] = {0, 0, 0};
2024-02-02 14:32:12 +01:00
double velSatI[3] = {0, 0, 0};
CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
2024-02-12 11:09:14 +01:00
VectorOperations<double>::cross(xAxisIX, velSatI, zAxisIX);
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
2022-12-13 11:51:03 +01:00
// y-Axis completes RHS
2024-02-12 11:09:14 +01:00
double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
2022-12-13 11:51:03 +01:00
2023-02-20 16:14:40 +01:00
// join transformation matrix
2024-02-12 11:09:14 +01:00
double dcmIX[3][3] = {{xAxisIX[0], yAxisIX[0], zAxisIX[0]},
{xAxisIX[1], yAxisIX[1], zAxisIX[1]},
{xAxisIX[2], yAxisIX[2], zAxisIX[2]}};
QuaternionOperations::fromDcm(dcmIX, targetQuat);
2022-12-13 11:51:03 +01:00
2024-02-02 14:32:12 +01:00
targetRotationRate(timeDelta, targetQuat, refSatRate);
}
void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], double *refSatRate) {
if (VectorOperations<double>::norm(quatIXprev, 4) == 0) {
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
}
if (timeDelta != 0.0) {
QuaternionOperations::rotationFromQuaternions(quatIX, quatIXprev, timeDelta, refSatRate);
} else {
std::memcpy(refSatRate, ZERO_VEC3, 3 * sizeof(double));
}
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
2022-11-22 21:10:05 +01:00
}
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
2024-02-05 09:24:41 +01:00
// First calculate error quaternion between current and target orientation without reference
// quaternion
double errorQuatWoRef[4] = {0, 0, 0, 0};
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
// Then add rotation from reference quaternion
QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
// Keep scalar part of quaternion positive
if (errorQuat[3] < 0) {
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
}
// Calculate error angle
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
2023-06-02 10:43:23 +02:00
// Calculate error satellite rotational rate
2024-01-26 13:16:27 +01:00
// Convert target rotational rate into body RF
double errorQuatInv[4] = {0, 0, 0, 0}, targetSatRotRateB[3] = {0, 0, 0};
QuaternionOperations::inverse(errorQuat, errorQuatInv);
2024-01-29 11:11:47 +01:00
QuaternionOperations::multiplyVector(errorQuatInv, targetSatRotRate, targetSatRotRateB);
2024-01-26 13:16:27 +01:00
// Combine the target and reference satellite rotational rates
2023-06-02 10:43:23 +02:00
double combinedRefSatRotRate[3] = {0, 0, 0};
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
// Then subtract the combined required satellite rotational rates from the actual rate
VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3);
2022-11-14 17:16:47 +01:00
}
2023-02-20 15:59:01 +01:00
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
double targetSatRotRate[3], double errorQuat[4],
double errorSatRotRate[3], double &errorAngle) {
double refQuat[4] = {0, 0, 0, 1}, refSatRotRate[3] = {0, 0, 0};
comparePtg(currentQuat, currentSatRotRate, targetQuat, targetSatRotRate, refQuat, refSatRotRate,
errorQuat, errorSatRotRate, errorAngle);
2022-09-19 15:17:39 +02:00
}
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
double *rwPseudoInv) {
2023-11-02 16:59:09 +01:00
bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid());
bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid());
bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid());
bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid());
2023-11-02 16:59:09 +01:00
if (rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double));
return returnvalue::OK;
2023-11-02 16:59:09 +01:00
} else if (not rw1valid and rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1,
12 * sizeof(double));
2024-01-29 11:08:47 +01:00
return acsctrl::SINGLE_RW_UNAVAILABLE;
2023-11-02 16:59:09 +01:00
} else if (rw1valid and not rw2valid and rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2,
12 * sizeof(double));
2024-01-29 11:08:47 +01:00
return acsctrl::SINGLE_RW_UNAVAILABLE;
2023-11-02 16:59:09 +01:00
} else if (rw1valid and rw2valid and not rw3valid and rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3,
12 * sizeof(double));
2024-01-29 11:08:47 +01:00
return acsctrl::SINGLE_RW_UNAVAILABLE;
2023-11-02 16:59:09 +01:00
} else if (rw1valid and rw2valid and rw3valid and not rw4valid) {
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4,
12 * sizeof(double));
2024-01-29 11:08:47 +01:00
return acsctrl::SINGLE_RW_UNAVAILABLE;
2022-10-12 15:06:24 +02:00
}
2024-01-29 11:08:47 +01:00
return acsctrl::MULTIPLE_RW_UNAVAILABLE;
2022-09-19 15:17:39 +02:00
}
2023-02-22 17:08:42 +01:00
void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
2023-04-14 11:37:23 +02:00
void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
2023-03-08 14:50:25 +01:00
std::error_code e;
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e) or
2023-03-10 10:30:01 +01:00
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
2023-03-10 09:44:30 +01:00
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDir,
2023-04-11 18:12:04 +02:00
3 * sizeof(double));
} else {
std::memcpy(sunTargetSafe, acsParameters->safeModeControllerParameters.sunTargetDirLeop,
2023-04-11 18:12:04 +02:00
3 * sizeof(double));
}
}
2023-02-22 17:08:42 +01:00
ReturnValue_t Guidance::solarArrayDeploymentComplete() {
2023-03-08 14:50:25 +01:00
std::error_code e;
if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) {
2023-02-22 17:08:42 +01:00
std::remove(SD_0_SKEWED_PTG_FILE);
2023-03-08 14:50:25 +01:00
if (std::filesystem::exists(SD_0_SKEWED_PTG_FILE, e)) {
2023-02-22 17:08:42 +01:00
return returnvalue::FAILED;
}
}
2023-03-08 14:50:25 +01:00
if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
2023-02-22 17:08:42 +01:00
std::remove(SD_1_SKEWED_PTG_FILE);
2023-03-08 14:50:25 +01:00
if (std::filesystem::exists(SD_1_SKEWED_PTG_FILE, e)) {
2023-02-22 17:08:42 +01:00
return returnvalue::FAILED;
}
}
return returnvalue::OK;
}