Rework PDEC FDIR #772
19
CHANGELOG.md
19
CHANGELOG.md
@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release:
|
||||
|
||||
# [unreleased]
|
||||
|
||||
- `eive-tmtc`:
|
||||
|
||||
## 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
|
||||
|
||||
@ -23,10 +29,17 @@ will consitute of a breaking change warranting a new major release:
|
||||
failed 10 times. The mechanism will reset after no PDEC reset has happended for 2 minutes.
|
||||
The PDEC reset will be performed when counting 4 dirty frame events 10 seconds after the count
|
||||
was incremented initially.
|
||||
- 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.
|
||||
|
||||
## Fixed
|
||||
- The handling function of the GPS data is only called once per GPS read. This should remove
|
||||
the fake fix-has-changed events.
|
||||
## Added
|
||||
|
||||
- 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
|
||||
|
||||
# [v6.3.0] 2023-08-03
|
||||
|
||||
|
||||
@ -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
2
fsfw
@ -1 +1 @@
|
||||
Subproject commit d575da85407e029dabecaffa5368f0c9f1034941
|
||||
Subproject commit 796c7a9e377fa197e7e79b9a757d1b8e97419b8f
|
||||
@ -21,6 +21,7 @@ GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, obj
|
||||
bool enableHkSets, bool debugHyperionGps)
|
||||
: ExtendedControllerBase(objectId),
|
||||
gpsSet(this),
|
||||
skyviewSet(this),
|
||||
enableHkSets(enableHkSets),
|
||||
debugHyperionGps(debugHyperionGps) {}
|
||||
|
||||
@ -29,7 +30,17 @@ GpsHyperionLinuxController::~GpsHyperionLinuxController() {
|
||||
gps_close(&gps);
|
||||
}
|
||||
|
||||
LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) { return &gpsSet; }
|
||||
LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) {
|
||||
switch (sid.ownerSetId) {
|
||||
case GpsHyperion::CORE_DATASET:
|
||||
return &gpsSet;
|
||||
case GpsHyperion::SKYVIEW_DATASET:
|
||||
return &skyviewSet;
|
||||
default:
|
||||
return nullptr;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t *msToReachTheMode) {
|
||||
@ -90,6 +101,13 @@ ReturnValue_t GpsHyperionLinuxController::initializeLocalDataPool(
|
||||
localDataPoolMap.emplace(GpsHyperion::SATS_IN_VIEW, new PoolEntry<uint8_t>());
|
||||
localDataPoolMap.emplace(GpsHyperion::FIX_MODE, new PoolEntry<uint8_t>());
|
||||
poolManager.subscribeForRegularPeriodicPacket({gpsSet.getSid(), enableHkSets, 30.0});
|
||||
localDataPoolMap.emplace(GpsHyperion::SKYVIEW_UNIX_SECONDS, new PoolEntry<double>());
|
||||
localDataPoolMap.emplace(GpsHyperion::PRN_ID, new PoolEntry<int16_t>());
|
||||
localDataPoolMap.emplace(GpsHyperion::AZIMUTH, new PoolEntry<int16_t>());
|
||||
localDataPoolMap.emplace(GpsHyperion::ELEVATION, new PoolEntry<int16_t>());
|
||||
localDataPoolMap.emplace(GpsHyperion::SIGNAL2NOISE, new PoolEntry<double>());
|
||||
localDataPoolMap.emplace(GpsHyperion::USED, new PoolEntry<uint8_t>());
|
||||
poolManager.subscribeForRegularPeriodicPacket({skyviewSet.getSid(), false, 120.0});
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
@ -210,7 +228,15 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
}
|
||||
ReturnValue_t result = handleCoreTelemetry(modeIsSet);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = handleSkyviewTelemetry();
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
|
||||
PoolReadGuard pg(&gpsSet);
|
||||
if (pg.getReadResult() != returnvalue::OK) {
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
@ -238,7 +264,9 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
||||
}
|
||||
}
|
||||
if (gpsSet.fixMode.value != newFix) {
|
||||
#if OBSW_Q7S_EM != 1
|
||||
triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFix);
|
||||
#endif
|
||||
}
|
||||
gpsSet.fixMode = newFix;
|
||||
gpsSet.fixMode.setValid(modeIsSet);
|
||||
@ -371,6 +399,22 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
ReturnValue_t GpsHyperionLinuxController::handleSkyviewTelemetry() {
|
||||
PoolReadGuard pg(&skyviewSet);
|
||||
if (pg.getReadResult() != returnvalue::OK) {
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
skyviewSet.unixSeconds.value = gps.skyview_time;
|
||||
for (int sat = 0; sat < GpsHyperion::MAX_SATELLITES; sat++) {
|
||||
skyviewSet.prn_id.value[sat] = gps.skyview[sat].PRN;
|
||||
skyviewSet.azimuth.value[sat] = gps.skyview[sat].azimuth;
|
||||
skyviewSet.elevation.value[sat] = gps.skyview[sat].elevation;
|
||||
skyviewSet.signal2noise.value[sat] = gps.skyview[sat].ss;
|
||||
skyviewSet.used.value[sat] = gps.skyview[sat].used;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) {
|
||||
if (not timeInit and validFix) {
|
||||
if (not utility::timeSanityCheck()) {
|
||||
|
||||
@ -54,9 +54,12 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
|
||||
LocalDataPoolManager& poolManager) override;
|
||||
|
||||
ReturnValue_t handleGpsReadData();
|
||||
ReturnValue_t handleCoreTelemetry(bool modeIsSet);
|
||||
ReturnValue_t handleSkyviewTelemetry();
|
||||
|
||||
private:
|
||||
GpsPrimaryDataset gpsSet;
|
||||
SkyviewDataset skyviewSet;
|
||||
gps_data_t gps = {};
|
||||
bool enableHkSets = false;
|
||||
const char* currentClientBuf = nullptr;
|
||||
|
||||
@ -20,32 +20,47 @@ static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity
|
||||
static constexpr DeviceCommandId_t GPS_REPLY = 0;
|
||||
static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;
|
||||
|
||||
static constexpr uint32_t DATASET_ID = 0;
|
||||
enum SetIds : uint32_t {
|
||||
CORE_DATASET,
|
||||
SKYVIEW_DATASET,
|
||||
};
|
||||
|
||||
enum GpsPoolIds : lp_id_t {
|
||||
LATITUDE = 0,
|
||||
LONGITUDE = 1,
|
||||
ALTITUDE = 2,
|
||||
SPEED = 3,
|
||||
FIX_MODE = 4,
|
||||
SATS_IN_USE = 5,
|
||||
SATS_IN_VIEW = 6,
|
||||
UNIX_SECONDS = 7,
|
||||
YEAR = 8,
|
||||
MONTH = 9,
|
||||
DAY = 10,
|
||||
HOURS = 11,
|
||||
MINUTES = 12,
|
||||
SECONDS = 13
|
||||
LATITUDE,
|
||||
LONGITUDE,
|
||||
ALTITUDE,
|
||||
SPEED,
|
||||
FIX_MODE,
|
||||
SATS_IN_USE,
|
||||
SATS_IN_VIEW,
|
||||
UNIX_SECONDS,
|
||||
YEAR,
|
||||
MONTH,
|
||||
DAY,
|
||||
HOURS,
|
||||
MINUTES,
|
||||
SECONDS,
|
||||
SKYVIEW_UNIX_SECONDS,
|
||||
PRN_ID,
|
||||
AZIMUTH,
|
||||
ELEVATION,
|
||||
SIGNAL2NOISE,
|
||||
USED,
|
||||
};
|
||||
|
||||
static constexpr uint8_t CORE_DATASET_ENTRIES = 14;
|
||||
static constexpr uint8_t SKYVIEW_ENTRIES = 6;
|
||||
|
||||
static constexpr uint8_t MAX_SATELLITES = 30;
|
||||
|
||||
enum GpsFixModes : uint8_t { INVALID = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3 };
|
||||
|
||||
} // namespace GpsHyperion
|
||||
|
||||
class GpsPrimaryDataset : public StaticLocalDataSet<18> {
|
||||
class GpsPrimaryDataset : public StaticLocalDataSet<GpsHyperion::CORE_DATASET_ENTRIES> {
|
||||
public:
|
||||
GpsPrimaryDataset(object_id_t gpsId) : StaticLocalDataSet(sid_t(gpsId, GpsHyperion::DATASET_ID)) {
|
||||
GpsPrimaryDataset(object_id_t gpsId)
|
||||
: StaticLocalDataSet(sid_t(gpsId, GpsHyperion::CORE_DATASET)) {
|
||||
setAllVariablesReadOnly();
|
||||
}
|
||||
|
||||
@ -69,7 +84,34 @@ class GpsPrimaryDataset : public StaticLocalDataSet<18> {
|
||||
friend class GpsHyperionLinuxController;
|
||||
friend class GpsCtrlDummy;
|
||||
GpsPrimaryDataset(HasLocalDataPoolIF* hkOwner)
|
||||
: StaticLocalDataSet(hkOwner, GpsHyperion::DATASET_ID) {}
|
||||
: StaticLocalDataSet(hkOwner, GpsHyperion::CORE_DATASET) {}
|
||||
};
|
||||
|
||||
class SkyviewDataset : public StaticLocalDataSet<GpsHyperion::SKYVIEW_ENTRIES> {
|
||||
public:
|
||||
SkyviewDataset(object_id_t gpsId)
|
||||
: StaticLocalDataSet(sid_t(gpsId, GpsHyperion::SKYVIEW_DATASET)) {
|
||||
setAllVariablesReadOnly();
|
||||
}
|
||||
|
||||
lp_var_t<double> unixSeconds =
|
||||
lp_var_t<double>(sid.objectId, GpsHyperion::SKYVIEW_UNIX_SECONDS, this);
|
||||
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES> prn_id =
|
||||
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::PRN_ID, this);
|
||||
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES> azimuth =
|
||||
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::AZIMUTH, this);
|
||||
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES> elevation =
|
||||
lp_vec_t<int16_t, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::ELEVATION, this);
|
||||
lp_vec_t<double, GpsHyperion::MAX_SATELLITES> signal2noise =
|
||||
lp_vec_t<double, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::SIGNAL2NOISE, this);
|
||||
lp_vec_t<uint8_t, GpsHyperion::MAX_SATELLITES> used =
|
||||
lp_vec_t<uint8_t, GpsHyperion::MAX_SATELLITES>(sid.objectId, GpsHyperion::USED, this);
|
||||
|
||||
private:
|
||||
friend class GpsHyperionLinuxController;
|
||||
friend class GpsCtrlDummy;
|
||||
SkyviewDataset(HasLocalDataPoolIF* hkOwner)
|
||||
: StaticLocalDataSet(hkOwner, GpsHyperion::SKYVIEW_DATASET) {}
|
||||
};
|
||||
|
||||
#endif /* MISSION_ACS_ARCHIVE_GPSDEFINITIONS_H_ */
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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,11 +167,18 @@ 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,
|
||||
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData, &acsParameters);
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||
@ -271,11 +299,18 @@ 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,
|
||||
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData, &acsParameters);
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||
@ -357,9 +392,16 @@ 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,
|
||||
result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData, &acsParameters);
|
||||
if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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_ */
|
||||
|
||||
@ -1,19 +1,5 @@
|
||||
#include "SensorProcessing.h"
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/timevalOperations.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
#include "Igrf13Model.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
using namespace Math;
|
||||
|
||||
SensorProcessing::SensorProcessing() {}
|
||||
|
||||
SensorProcessing::~SensorProcessing() {}
|
||||
@ -24,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) {
|
||||
{
|
||||
@ -54,6 +41,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
|
||||
}
|
||||
}
|
||||
std::memcpy(savedMgmVecTot, ZERO_VEC_D, sizeof(savedMgmVecTot));
|
||||
return;
|
||||
}
|
||||
float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
|
||||
@ -141,14 +129,14 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0};
|
||||
bool mgmVecTotDerivativeValid = false;
|
||||
double timeDiff = timevalOperations::toDouble(timeOfMgmMeasurement - timeOfSavedMagFieldEst);
|
||||
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
|
||||
savedMgmVecTot[i] = mgmVecTot[i];
|
||||
if (timeOfSavedMagFieldEst.tv_sec != 0 and timeDiff > 0 and
|
||||
VectorOperations<double>::norm(savedMgmVecTot, 3) != 0) {
|
||||
VectorOperations<double>::subtract(mgmVecTot, savedMgmVecTot, mgmVecTotDerivative, 3);
|
||||
VectorOperations<double>::mulScalar(mgmVecTotDerivative, 1. / timeDiff, mgmVecTotDerivative, 3);
|
||||
mgmVecTotDerivativeValid = true;
|
||||
}
|
||||
}
|
||||
timeOfSavedMagFieldEst = timeOfMgmMeasurement;
|
||||
std::memcpy(savedMgmVecTot, mgmVecTot, sizeof(savedMgmVecTot));
|
||||
|
||||
if (VectorOperations<double>::norm(mgmVecTotDerivative, 3) != 0 and
|
||||
mgmDataProcessed->mgmVecTotDerivative.isValid()) {
|
||||
@ -199,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);
|
||||
@ -277,6 +265,7 @@ void SensorProcessing::processSus(
|
||||
susDataProcessed->sunIjkModel.setValid(true);
|
||||
}
|
||||
}
|
||||
std::memcpy(savedSusVecTot, ZERO_VEC_D, sizeof(savedSusVecTot));
|
||||
return;
|
||||
}
|
||||
|
||||
@ -365,13 +354,13 @@ void SensorProcessing::processSus(
|
||||
double susVecTotDerivative[3] = {0.0, 0.0, 0.0};
|
||||
bool susVecTotDerivativeValid = false;
|
||||
double timeDiff = timevalOperations::toDouble(timeOfSusMeasurement - timeOfSavedSusDirEst);
|
||||
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0) {
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
susVecTotDerivative[i] = (susVecTot[i] - savedSusVecTot[i]) / timeDiff;
|
||||
savedSusVecTot[i] = susVecTot[i];
|
||||
if (timeOfSavedSusDirEst.tv_sec != 0 and timeDiff > 0 and
|
||||
VectorOperations<double>::norm(savedSusVecTot, 3) != 0) {
|
||||
VectorOperations<double>::subtract(susVecTot, savedSusVecTot, susVecTotDerivative, 3);
|
||||
VectorOperations<double>::mulScalar(susVecTotDerivative, 1. / timeDiff, susVecTotDerivative, 3);
|
||||
susVecTotDerivativeValid = true;
|
||||
}
|
||||
}
|
||||
std::memcpy(savedSusVecTot, susVecTot, sizeof(savedSusVecTot));
|
||||
if (VectorOperations<double>::norm(susVecTotDerivative, 3) != 0 and
|
||||
susDataProcessed->susVecTotDerivative.isValid()) {
|
||||
lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value,
|
||||
@ -535,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
|
||||
@ -569,6 +575,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
|
||||
|
||||
timeOfSavedPosSatE = gpsUnixSeconds;
|
||||
validSavedPosSatE = true;
|
||||
|
||||
gpsSource = acs::GpsSource::GPS;
|
||||
}
|
||||
{
|
||||
PoolReadGuard pg(gpsDataProcessed);
|
||||
@ -579,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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -606,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(),
|
||||
|
||||
@ -1,15 +1,23 @@
|
||||
#ifndef SENSORPROCESSING_H_
|
||||
#define SENSORPROCESSING_H_
|
||||
|
||||
#include <common/config/eive/resultClassIds.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/timevalOperations.h>
|
||||
#include <fsfw/returnvalues/returnvalue.h>
|
||||
#include <stdint.h> //uint8_t
|
||||
#include <time.h> /*purpose, timeval ?*/
|
||||
#include <mission/acs/defs.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/Igrf13Model.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <mission/controller/acs/SusConverter.h>
|
||||
#include <mission/controller/acs/util/MathOperations.h>
|
||||
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
|
||||
|
||||
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
#include "AcsParameters.h"
|
||||
#include "SensorValues.h"
|
||||
#include "SusConverter.h"
|
||||
#include "eive/resultClassIds.h"
|
||||
#include <cmath>
|
||||
|
||||
class SensorProcessing {
|
||||
public:
|
||||
@ -25,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
|
||||
@ -32,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,
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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_ */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user