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

CFDP source handler #776

Merged
muellerr merged 96 commits from cfdp-source-handler into main 2023-10-19 11:11:31 +02:00
Conversation 0 Commits 96 Files Changed 56 +235 -39
14 changed files with 235 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 288b09766d - Show all commits

6
CHANGELOG.md
View File

@ -20,13 +20,19 @@ will consitute of a breaking change warranting a new major release:
## Added
meggert commented 2023-10-18 10:51:33 +02:00
Outdated
Review
Copy Link

he, she, it - the s must fit

he, she, it - the s must fit
- SGP4 Propagator is now used for propagating the position of EIVE. It will only work once
a TLE has been uploaded with the newly added action command for the ACS Controller. In
return the actual GPS data will be ignored once SPG4 is running. However, by setting the
according parameter, the ACS Controller can be directed to ignore the SGP4 solution.
- Skyview dataset for more GPS TM has been added
## Fixed
- The handling function of the GPS data is only called once per GPS read. This should remove
the fake fix-has-changed events.
## Changed
- GPS Fix has changed event is no longer triggered for the EM
- MGM and SUS rates now will only be calculated, if 2 valid consecutive datapoints are available.
The stored value of the last timestep will now be reset, if no actual value is available.

3
CMakeLists.txt
View File

@ -240,6 +240,9 @@ set(FSFW_WARNING_SHADOW_LOCAL_GCC OFF)
set(EIVE_ADD_LINUX_FILES OFF)
set(FSFW_ADD_TMSTORAGE ON)
set(FSFW_ADD_COORDINATES ON)
set(FSFW_ADD_SGP4_PROPAGATOR ON)
# Analyse different OS and architecture/target options, determine BSP_PATH,
# display information about compiler etc.
pre_source_hw_os_config()

2
fsfw

@ -1 +1 @@
Subproject commit 036667a969c82272eeb65adfdb068e34e318ef75
Subproject commit b39e1c7e076914d18fbb78716d3b5b9b12b8504b

9
mission/acs/defs.h
View File

@ -42,6 +42,13 @@ enum SafeModeStrategy : uint8_t {
SAFECTRL_DETUMBLE_DETERIORATED = 21,
};
enum GpsSource : uint8_t {
NONE,
GPS,
GPS_EXTRAPOLATED,
SPG4,
};
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
//! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
@ -64,6 +71,8 @@ static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIG
//! failed.
//! P1: Missing information about magnetic field, P2: Missing information about rotational rate
static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH);
//! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old.
static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO);
extern const char* getModeStr(AcsMode mode);

60
mission/controller/AcsController.cpp
View File

@ -22,7 +22,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
mekfData(this),
ctrlValData(this),
actuatorCmdData(this),
fusedRotRateData(this) {}
fusedRotRateData(this),
tleData(this) {}
ReturnValue_t AcsController::initialize() {
ReturnValue_t result = parameterHelper.initialize();
@ -62,6 +63,26 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
mekfLost = false;
return HasActionsIF::EXECUTION_FINISHED;
}
case UPDATE_TLE: {
if (size != 69 * 2) {
return INVALID_PARAMETERS;
}
ReturnValue_t result = navigation.updateTle(data, data + 69);
if (result != returnvalue::OK) {
PoolReadGuard pg(&tleData);
navigation.updateTle(tleData.line1.value, tleData.line2.value);
return result;
}
{
PoolReadGuard pg(&tleData);
if (pg.getReadResult() == returnvalue::OK) {
std::memcpy(tleData.line1.value, data, 69);
std::memcpy(tleData.line2.value, data + 69, 69);
tleData.setValidity(true, true);
}
}
return HasActionsIF::EXECUTION_FINISHED;
}
default: {
return HasActionsIF::INVALID_ACTION_ID;
}
@ -146,12 +167,19 @@ void AcsController::performSafe() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else if (result != Sgp4Propagator::TLE_TOO_OLD) {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -271,12 +299,19 @@ void AcsController::performDetumble() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
&gyrDataProcessed, &fusedRotRateData);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
if (not mekfInvalidFlag) {
@ -357,10 +392,17 @@ void AcsController::performPointingCtrl() {
timeval now;
Clock::getClock_timeval(&now);
ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
triggerEvent(acs::TLE_TOO_OLD);
tleTooOldFlag = true;
} else {
tleTooOldFlag = false;
}
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
&susDataProcessed, &mekfData, &acsParameters);
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
mekfInvalidCounter++;
@ -735,6 +777,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
// MEKF
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
@ -758,6 +801,9 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
// TLE Data
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1);
localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2);
return returnvalue::OK;
}

9
mission/controller/AcsController.h
View File

@ -3,6 +3,7 @@
#include <eive/objects.h>
#include <fsfw/controller/ExtendedControllerBase.h>
#include <fsfw/coordinates/Sgp4Propagator.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/health/HealthTable.h>
#include <fsfw/parameters/ParameterHelper.h>
@ -61,6 +62,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ParameterHelper parameterHelper;
bool tleTooOldFlag = false;
uint8_t detumbleCounter = 0;
uint8_t multipleRwUnavailableCounter = 0;
bool mekfInvalidFlag = false;
@ -84,6 +86,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
static const DeviceCommandId_t RESET_MEKF = 0x1;
static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
static const DeviceCommandId_t UPDATE_TLE = 0x3;
static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
//! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
@ -207,6 +210,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> altitude = PoolEntry<double>();
PoolEntry<double> gpsPosition = PoolEntry<double>(3);
PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
// MEKF
acsctrl::MekfData mekfData;
@ -234,6 +238,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
// TLE Dataset
acsctrl::TleData tleData;
PoolEntry<uint8_t> line1 = PoolEntry<uint8_t>(69);
PoolEntry<uint8_t> line2 = PoolEntry<uint8_t>(69);
// Initial delay to make sure all pool variables have been initialized their owners
Countdown initialCountdown = Countdown(INIT_DELAY);
};

3
mission/controller/acs/AcsParameters.cpp
View File

@ -665,6 +665,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case 0x3:
parameterWrapper->set(gpsParameters.fdirAltitude);
break;
case 0x4:
parameterWrapper->set(gpsParameters.useSpg4);
break;
default:
return INVALID_IDENTIFIER_ID;
}

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

@ -916,6 +916,7 @@ class AcsParameters : public HasParametersIF {
double minimumFdirAltitude = 475 * 1e3; // [m]
double maximumFdirAltitude = 575 * 1e3; // [m]
double fdirAltitude = 525 * 1e3; // [m]
uint8_t useSpg4 = true;
} gpsParameters;
struct SunModelParameters {

37
mission/controller/acs/Navigation.cpp
View File

@ -44,3 +44,40 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
}
ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) {
double position[3] = {0, 0, 0};
double velocity[3] = {0, 0, 0};
ReturnValue_t result = sgp4Propagator.propagate(position, velocity, now, 0);
if (result == returnvalue::OK) {
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->source = acs::GpsSource::SPG4;
gpsDataProcessed->source.setValid(true);
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
gpsDataProcessed->gpsPosition.setValid(true);
std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double));
gpsDataProcessed->gpsVelocity.setValid(true);
}
}
} else {
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->source = acs::GpsSource::NONE;
gpsDataProcessed->source.setValid(true);
std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
gpsDataProcessed->gpsPosition.setValid(false);
std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double));
gpsDataProcessed->gpsVelocity.setValid(false);
}
}
}
return result;
}
ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
return sgp4Propagator.initialize(line1, line2);
}

16
mission/controller/acs/Navigation.h
View File

@ -1,11 +1,13 @@
#ifndef NAVIGATION_H_
#define NAVIGATION_H_
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
#include "AcsParameters.h"
#include "MultiplicativeKalmanFilter.h"
#include "SensorProcessing.h"
#include "SensorValues.h"
#include <fsfw/coordinates/Sgp4Propagator.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
#include <mission/controller/acs/SensorProcessing.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
class Navigation {
public:
@ -19,10 +21,14 @@ class Navigation {
AcsParameters *acsParameters);
void resetMekf(acsctrl::MekfData *mekfData);
ReturnValue_t useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
protected:
private:
MultiplicativeKalmanFilter multiplicativeKalmanFilter;
ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
Sgp4Propagator sgp4Propagator;
};
#endif /* ACS_NAVIGATION_H_ */

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

@ -10,19 +10,20 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
bool mgm4valid, timeval timeOfMgmMeasurement,
const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed,
const double gpsAltitude, bool gpsValid,
acsctrl::MgmDataProcessed *mgmDataProcessed) {
// ---------------- IGRF- 13 Implementation here
// ------------------------------------------------
double magIgrfModel[3] = {0.0, 0.0, 0.0};
if (gpsValid) {
bool gpsValid = false;
if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
Igrf13Model igrf13;
igrf13.schmidtNormalization();
igrf13.updateCoeffGH(timeOfMgmMeasurement);
// maybe put a condition here, to only update after a full day, this
// class function has around 700 steps to perform
igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel);
gpsValid = true;
}
if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
{
@ -186,8 +187,8 @@ void SensorProcessing::processSus(
double JC2000 = JD2000 / 36525.;
double meanLongitude =
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * M_PI / 180.;
double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * M_PI / 180.;
double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
sunModelParameters->p2 * sin(2 * meanAnomaly);
@ -523,14 +524,31 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
const bool validGps,
const AcsParameters::GpsParameters *gpsParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed) {
double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
// init variables
double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
gpsVelocityE[3] = {0, 0, 0};
if (validGps) {
// Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
gdLongitude = gpsLongitude * PI / 180.;
double latitudeRad = gpsLatitude * PI / 180.;
double eccentricityWgs84 = 0.0818195;
double factor = 1 - pow(eccentricityWgs84, 2);
uint8_t gpsSource = acs::GpsSource::NONE;
// We do not trust the GPS and therefore it shall die here if SPG4 is running
if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) {
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
gdLongitude, altitude);
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
gcLatitude = atan(factor * tan(gdLatitude));
{
PoolReadGuard pg(gpsDataProcessed);
if (pg.getReadResult() == returnvalue::OK) {
gpsDataProcessed->gdLongitude.value = gdLongitude;
gpsDataProcessed->gcLatitude.value = gcLatitude;
gpsDataProcessed->altitude.value = altitude;
gpsDataProcessed->setValidity(true, true);
}
}
return;
} else if (validGps) {
// Transforming from Degree to Radians and calculation geocentric latitude from geodetic
gdLongitude = gpsLongitude * M_PI / 180.;
double latitudeRad = gpsLatitude * M_PI / 180.;
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
gcLatitude = atan(factor * tan(latitudeRad));
// Altitude FDIR
@ -557,6 +575,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
timeOfSavedPosSatE = gpsUnixSeconds;
validSavedPosSatE = true;
gpsSource = acs::GpsSource::GPS;
}
{
PoolReadGuard pg(gpsDataProcessed);
@ -567,6 +587,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
gpsDataProcessed->setValidity(validGps, true);
gpsDataProcessed->source.value = gpsSource;
gpsDataProcessed->source.setValid(true);
}
}
}
@ -594,11 +616,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
sensorValues->mgm3Rm3100Set.fieldStrengths.value,
sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
sensorValues->gpsSet.altitude.value,
(sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
sensorValues->gpsSet.altitude.isValid()),
mgmDataProcessed);
now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed);
processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),

6
mission/controller/acs/SensorProcessing.h
View File

@ -9,6 +9,7 @@
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/timevalOperations.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <mission/acs/defs.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/Igrf13Model.h>
#include <mission/controller/acs/SensorValues.h>
@ -32,6 +33,7 @@ class SensorProcessing {
private:
static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
protected:
// short description needed for every function
@ -39,8 +41,8 @@ class SensorProcessing {
const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
const AcsParameters::MgmHandlingParameters *mgmParameters,
acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude,
bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed);
acsctrl::GpsDataProcessed *gpsDataProcessed,
acsctrl::MgmDataProcessed *mgmDataProcessed);
void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value,
bool sus1valid, const uint16_t *sus2Value, bool sus2valid,

51
mission/controller/acs/util/MathOperations.h
View File

@ -3,14 +3,15 @@
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <math.h>
#include <fsfw/src/fsfw/globalfunctions/sign.h>
#include <stdint.h>
#include <string.h>
#include <sys/time.h>
#include <cmath>
#include <iostream>
using namespace Math;
#include "fsfw/serviceinterface.h"
template <typename T1, typename T2 = T1>
class MathOperations {
@ -114,6 +115,44 @@ class MathOperations {
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
}
static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
/* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from
* cartesian coordinates in ECEF
* @param: x x-value of position vector [m]
* y y-value of position vector [m]
* z z-value of position vector [m]
* latitude geodetic latitude [rad]
* longitude longitude [rad]
* altitude altitude [m]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
* Landis Markley and John L. Crassidis*/
// From World Geodetic System the Earth Radii
double a = 6378137.0; // semimajor axis [m]
double b = 6356752.3142; // semiminor axis [m]
// Calculation
double e2 = 1 - pow(b, 2) / pow(a, 2);
double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
double p = std::abs(vector[2]) / epsilon2;
double s = pow(rho, 2) / (e2 * epsilon2);
double q = pow(p, 2) - pow(b, 2) + s;
double u = p / sqrt(q);
double v = pow(b, 2) * pow(u, 2) / q;
double P = 27 * v * s / q;
double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
double t = (1 + Q + 1 / Q) / 6;
double c = sqrt(pow(u, 2) - 1 + 2 * t);
double w = (c - u) / 2;
double d =
sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
latitude = asin((epsilon2 + 1) * d / N);
altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
longitude = atan2(vector[1], vector[0]);
}
static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
* @param: time Current time
@ -140,7 +179,7 @@ class MathOperations {
double FloorRest = floor(rest);
double secOfDay = rest - FloorRest;
secOfDay *= 86400;
gmst = secOfDay / 240 * PI / 180;
gmst = secOfDay / 240 * M_PI / 180;
outputDcmEJ[0] = cos(gmst);
outputDcmEJ[1] = sin(gmst);
@ -191,11 +230,11 @@ class MathOperations {
double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// Earth Rotation angle
double era = 0;
era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
// Greenwich Mean Sidereal Time
double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
double arcsecFactor = 1 * PI / (180 * 3600);
double arcsecFactor = 1 * M_PI / (180 * 3600);
gmst2000 *= arcsecFactor;
gmst2000 += era;
@ -247,7 +286,7 @@ class MathOperations {
double de = 9.203 * arcsecFactor * cos(Om);
// % true obliquity of the ecliptic eps p.71 (simplified)
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;
nutation[0][0] = cos(dp);
nutation[1][0] = cos(e + de) * sin(dp);

19
mission/controller/controllerdefinitions/AcsCtrlDefinitions.h
View File

@ -20,6 +20,7 @@ enum SetIds : uint32_t {
CTRL_VAL_DATA,
ACTUATOR_CMD_DATA,
FUSED_ROTATION_RATE_DATA,
TLE_SET,
};
enum PoolIds : lp_id_t {
@ -85,6 +86,7 @@ enum PoolIds : lp_id_t {
GYR_3_VEC,
GYR_VEC_TOT,
// GPS Processed
SOURCE,
GC_LATITUDE,
GD_LONGITUDE,
ALTITUDE,
@ -108,6 +110,9 @@ enum PoolIds : lp_id_t {
ROT_RATE_ORTHOGONAL,
ROT_RATE_PARALLEL,
ROT_RATE_TOTAL,
// TLE
TLE_LINE_1,
TLE_LINE_2,
};
static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@ -116,11 +121,12 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
static constexpr uint8_t MEKF_SET_ENTRIES = 3;
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
static constexpr uint8_t TLE_SET_ENTRIES = 2;
/**
* @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@ -239,6 +245,7 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, ALTITUDE, this);
lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);
lp_var_t<uint8_t> source = lp_var_t<uint8_t>(sid.objectId, SOURCE, this);
private:
};
@ -292,6 +299,16 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
private:
};
class TleData : public StaticLocalDataSet<TLE_SET_ENTRIES> {
public:
TleData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, TLE_SET) {}
lp_vec_t<uint8_t, 69> line1 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
lp_vec_t<uint8_t, 69> line2 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
private:
};
} // namespace acsctrl
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */