eive/eive-obsw
eive
/
eive-obsw
12
3
Fork 0
Code Issues 3 Pull Requests 1 Releases 117 Wiki Activity

Merge branch 'main' into fix-desaturation
EIVE/eive-obsw/pipeline/pr-main This commit looks good Details

This commit is contained in:
Marius Eggert 2024-02-28 09:23:30 +01:00
parent 64f6f92ce6 61555aa5cf
commit a2a360c1d7
23 changed files with 444 additions and 791 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
CHANGELOG.md
View File

@ -16,6 +16,9 @@ will consitute of a breaking change warranting a new major release:
# [unreleased]
- Bumped `eive-tmtc` to
- Bumped `eive-fsfw`
## Fixed
- PLOC SUPV sets: Added missing `PoolReadGuard` instantiations when reading boot status report
@ -32,6 +35,17 @@ will consitute of a breaking change warranting a new major release:
- `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes
- QUEST and STR rates are now allowed per default
- Changed PTG Strat priorities to favor STR before MEKF.
- Increased message queue depth and maximum number of handled messages per cycle for
`PusServiceBase` based classes (especially PUS scheduler).
- `MathOperations` functions were moved to their appropriate classes within the `eive-fsfw`
- Changed pointing strategy for target groundstation mode to prevent blinding of the STR. This
also limits the rotation for the reference target quaternion to prevent spikes in required
rotation rates.
- Updated QUEST and Sun Vector Params to new values.
## Added
- Updated STR handler to unlock and allow using the secondary firmware slot.
# [v7.6.1] 2024-02-05

2
bsp_q7s/objectFactory.cpp
View File

@ -951,7 +951,7 @@ void ObjectFactory::createStrComponents(PowerSwitchIF* pwrSwitcher, SdCardManage
auto cfgGetter = new StrConfigPathGetter(sdcMan);
auto starTracker =
new StarTrackerHandler(objects::STAR_TRACKER, objects::STR_COM_IF, starTrackerCookie,
strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter);
strComIF, power::PDU1_CH2_STAR_TRACKER_5V, *cfgGetter, sdcMan);
starTracker->setPowerSwitcher(pwrSwitcher);
starTracker->connectModeTreeParent(*strAssy);
starTracker->setCustomFdir(strFdir);

2
fsfw

@ -1 +1 @@
Subproject commit b5e7179af1da085b9be598b4897f2664012781af
Subproject commit 516357d855c07786b492e981230988186376d301

40
linux/acs/StrComHandler.cpp
View File

@ -175,7 +175,8 @@ void StrComHandler::setDownloadImageName(std::string filename) {
void StrComHandler::setFlashReadFilename(std::string filename) { flashRead.filename = filename; }
ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) {
ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname,
startracker::FirmwareTarget target) {
{
MutexGuard mg(lock);
if (state != InternalState::SLEEPING) {
@ -192,8 +193,13 @@ ReturnValue_t StrComHandler::startFirmwareUpdate(std::string fullname) {
if (not std::filesystem::exists(flashWrite.fullname)) {
return FILE_NOT_EXISTS;
}
flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST);
flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST);
if (target == startracker::FirmwareTarget::MAIN) {
flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_MAIN);
flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_MAIN);
} else if (target == startracker::FirmwareTarget::BACKUP) {
flashWrite.firstRegion = static_cast<uint8_t>(startracker::FirmwareRegions::FIRST_BACKUP);
flashWrite.lastRegion = static_cast<uint8_t>(startracker::FirmwareRegions::LAST_BACKUP);
}
{
MutexGuard mg(lock);
replyWasReceived = false;
@ -275,7 +281,7 @@ ReturnValue_t StrComHandler::performImageDownload() {
file.close();
return result;
}
result = checkActionReply(replySize);
result = checkActionReply(replySize, "downloading image");
if (result != returnvalue::OK) {
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
serial::flushRxBuf(serialPort);
@ -348,7 +354,7 @@ ReturnValue_t StrComHandler::performImageUpload() {
if (result != returnvalue::OK) {
return returnvalue::FAILED;
}
result = checkActionReply(replyLen);
result = checkActionReply(replyLen, "sky image upload");
if (result != returnvalue::OK) {
return result;
}
@ -374,7 +380,7 @@ ReturnValue_t StrComHandler::performImageUpload() {
if (result != returnvalue::OK) {
return returnvalue::FAILED;
}
result = checkActionReply(replyLen);
result = checkActionReply(replyLen, "sky image upload");
if (result != returnvalue::OK) {
return result;
}
@ -388,8 +394,7 @@ ReturnValue_t StrComHandler::performImageUpload() {
ReturnValue_t StrComHandler::performFirmwareUpdate() {
using namespace startracker;
ReturnValue_t result = returnvalue::OK;
result = unlockAndEraseRegions(static_cast<uint32_t>(startracker::FirmwareRegions::FIRST),
static_cast<uint32_t>(startracker::FirmwareRegions::LAST));
result = unlockAndEraseRegions(flashWrite.firstRegion, flashWrite.lastRegion);
if (result != returnvalue::OK) {
return result;
}
@ -445,7 +450,7 @@ ReturnValue_t StrComHandler::performFlashWrite() {
if (result != returnvalue::OK) {
return result;
}
result = checkActionReply(replyLen);
result = checkActionReply(replyLen, "firmware image upload");
if (result != returnvalue::OK) {
return result;
}
@ -488,7 +493,7 @@ ReturnValue_t StrComHandler::performFlashWrite() {
if (result != returnvalue::OK) {
return result;
}
result = checkActionReply(replyLen);
result = checkActionReply(replyLen, "flash write");
if (result != returnvalue::OK) {
return result;
}
@ -542,7 +547,7 @@ ReturnValue_t StrComHandler::performFlashRead() {
file.close();
return result;
}
result = checkActionReply(replyLen);
result = checkActionReply(replyLen, "flash read");
if (result != returnvalue::OK) {
if (retries < CONFIG_MAX_DOWNLOAD_RETRIES) {
serial::flushRxBuf(serialPort);
@ -584,7 +589,7 @@ ReturnValue_t StrComHandler::sendAndRead(size_t size, uint32_t failParameter) {
return readOneReply(failParameter);
}
ReturnValue_t StrComHandler::checkActionReply(size_t replySize) {
ReturnValue_t StrComHandler::checkActionReply(size_t replySize, const char* context) {
uint8_t type = startracker::getReplyFrameType(replyPtr);
if (type != TMTC_ACTIONREPLY) {
sif::warning << "StrHelper::checkActionReply: Received reply with invalid type ID" << std::endl;
@ -592,7 +597,7 @@ ReturnValue_t StrComHandler::checkActionReply(size_t replySize) {
}
uint8_t status = startracker::getStatusField(replyPtr);
if (status != ArcsecDatalinkLayer::STATUS_OK) {
sif::warning << "StrHelper::checkActionReply: Status failure: "
sif::warning << "StrHelper::checkActionReply: Status failure for " << context << ": "
<< static_cast<unsigned int>(status) << std::endl;
return STATUS_ERROR;
}
@ -744,15 +749,15 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) {
struct UnlockActionRequest unlockReq;
struct EraseActionRequest eraseReq;
uint32_t size = 0;
for (uint32_t idx = from; idx <= to; idx++) {
for (uint32_t idx = from; idx < to; idx++) {
unlockReq.region = idx;
unlockReq.code = startracker::region_secrets::secret[idx];
unlockReq.code = startracker::region_secrets::SECRETS[idx];
arc_pack_unlock_action_req(&unlockReq, cmdBuf.data(), &size);
result = sendAndRead(size, unlockReq.region);
if (result != returnvalue::OK) {
return result;
}
result = checkActionReply(replyLen);
result = checkActionReply(replyLen, "unlocking region");
if (result != returnvalue::OK) {
sif::warning << "StrHelper::unlockAndEraseRegions: Failed to unlock region with id "
<< static_cast<unsigned int>(unlockReq.region) << std::endl;
@ -761,6 +766,9 @@ ReturnValue_t StrComHandler::unlockAndEraseRegions(uint32_t from, uint32_t to) {
eraseReq.region = idx;
arc_pack_erase_action_req(&eraseReq, cmdBuf.data(), &size);
result = sendAndRead(size, eraseReq.region);
if (result != returnvalue::OK) {
}
result = checkActionReply(replyLen, "erasing region");
if (result != returnvalue::OK) {
sif::warning << "StrHelper::unlockAndEraseRegions: Failed to erase region with id "
<< static_cast<unsigned int>(eraseReq.region) << std::endl;

5
linux/acs/StrComHandler.h
View File

@ -6,6 +6,7 @@
#include <string>
#include "OBSWConfig.h"
#include "mission/acs/str/strHelpers.h"
#ifdef XIPHOS_Q7S
#include "bsp_q7s/fs/SdCardManager.h"
@ -127,7 +128,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public
* @param fullname Full name including absolute path of file containing firmware
* update.
*/
ReturnValue_t startFirmwareUpdate(std::string fullname);
ReturnValue_t startFirmwareUpdate(std::string fullname, startracker::FirmwareTarget target);
/**
* @brief Starts the flash read procedure
@ -334,7 +335,7 @@ class StrComHandler : public SystemObject, public DeviceCommunicationIF, public
*
* @return returnvalue::OK if reply confirms success of packet transfer, otherwise REUTRN_FAILED
*/
ReturnValue_t checkActionReply(size_t replySize);
ReturnValue_t checkActionReply(size_t replySize, const char *context);
/**
* @brief Checks the position field in a star tracker upload/download reply.

126
mission/acs/str/StarTrackerHandler.cpp
View File

@ -5,7 +5,12 @@
#include <mission/acs/str/strHelpers.h>
#include <mission/acs/str/strJsonCommands.h>
#include <string>
#include "fsfw/filesystem/HasFileSystemIF.h"
#include "fsfw/ipc/MessageQueueIF.h"
#include "fsfw/returnvalues/returnvalue.h"
#include "mission/memory/SdCardMountedIF.h"
extern "C" {
#include <sagitta/client/actionreq.h>
@ -16,7 +21,6 @@ extern "C" {
}
#include <atomic>
#include <fstream>
#include <thread>
#include "OBSWConfig.h"
@ -27,7 +31,8 @@ std::atomic_bool JCFG_DONE(false);
StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
StrComHandler* strHelper, power::Switch_t powerSwitch,
startracker::SdCardConfigPathGetter& cfgPathGetter)
startracker::SdCardConfigPathGetter& cfgPathGetter,
SdCardMountedIF& sdCardIF)
: DeviceHandlerBase(objectId, comIF, comCookie),
temperatureSet(this),
versionSet(this),
@ -60,6 +65,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF,
contrastSet(this),
strHelper(strHelper),
powerSwitch(powerSwitch),
sdCardIF(sdCardIF),
cfgPathGetter(cfgPathGetter) {
if (comCookie == nullptr) {
sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl;
@ -138,6 +144,9 @@ ReturnValue_t StarTrackerHandler::initialize() {
// delay whole satellite boot process.
reloadJsonCfgFile();
// Default firmware target is always initialized from persistent file.
loadTargetFirmwareFromPersistentCfg();
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
if (manager == nullptr) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
@ -165,6 +174,19 @@ ReturnValue_t StarTrackerHandler::initialize() {
return returnvalue::OK;
}
void StarTrackerHandler::loadTargetFirmwareFromPersistentCfg() {
const char* prefix = sdCardIF.getCurrentMountPrefix();
std::filesystem::path path = std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
std::ifstream ifile(path);
if (ifile.is_open() and !ifile.bad()) {
std::string targetStr;
std::getline(ifile, targetStr);
if (targetStr == "backup") {
firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP);
}
}
}
bool StarTrackerHandler::reloadJsonCfgFile() {
jcfgCountdown.resetTimer();
auto strCfgPath = cfgPathGetter.getCfgPath();
@ -307,21 +329,11 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
strHelper->setFlashReadFilename(std::string(reinterpret_cast<const char*>(data), size));
return EXECUTION_FINISHED;
}
case (startracker::FIRMWARE_UPDATE): {
result = DeviceHandlerBase::acceptExternalDeviceCommands();
if (result != returnvalue::OK) {
return result;
}
if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
return FILE_PATH_TOO_LONG;
}
result =
strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size));
if (result != returnvalue::OK) {
return result;
}
strHelperHandlingSpecialRequest = true;
return EXECUTION_FINISHED;
case (startracker::FIRMWARE_UPDATE_MAIN): {
return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::MAIN);
}
case (startracker::FIRMWARE_UPDATE_BACKUP): {
return handleFirmwareUpdateCommand(data, size, startracker::FirmwareTarget::BACKUP);
}
default:
break;
@ -330,6 +342,23 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
reinitNextSetParam = true;
return DeviceHandlerBase::executeAction(actionId, commandedBy, data, size);
}
ReturnValue_t StarTrackerHandler::handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
startracker::FirmwareTarget target) {
ReturnValue_t result = DeviceHandlerBase::acceptExternalDeviceCommands();
if (result != returnvalue::OK) {
return result;
}
if (size > config::MAX_PATH_SIZE + config::MAX_FILENAME_SIZE) {
return FILE_PATH_TOO_LONG;
}
result = strHelper->startFirmwareUpdate(std::string(reinterpret_cast<const char*>(data), size),
target);
if (result != returnvalue::OK) {
return result;
}
strHelperHandlingSpecialRequest = true;
return EXECUTION_FINISHED;
}
void StarTrackerHandler::performOperationHook() {
EventMessage event;
@ -569,7 +598,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
return returnvalue::OK;
}
case (startracker::BOOT): {
prepareBootCommand();
prepareBootCommand(static_cast<startracker::FirmwareTarget>(firmwareTargetRaw));
return returnvalue::OK;
}
case (startracker::REQ_VERSION): {
@ -1659,7 +1688,8 @@ ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) {
case startracker::UPLOAD_IMAGE:
case startracker::DOWNLOAD_IMAGE:
case startracker::FLASH_READ:
case startracker::FIRMWARE_UPDATE: {
case startracker::FIRMWARE_UPDATE_BACKUP:
case startracker::FIRMWARE_UPDATE_MAIN: {
return DeviceHandlerBase::acceptExternalDeviceCommands();
default:
break;
@ -1956,9 +1986,9 @@ ReturnValue_t StarTrackerHandler::executeFlashReadCommand(const uint8_t* command
return result;
}
void StarTrackerHandler::prepareBootCommand() {
void StarTrackerHandler::prepareBootCommand(startracker::FirmwareTarget target) {
uint32_t length = 0;
struct BootActionRequest bootRequest = {BOOT_REGION_ID};
struct BootActionRequest bootRequest = {static_cast<uint8_t>(target)};
arc_pack_boot_action_req(&bootRequest, commandBuffer, &length);
rawPacket = commandBuffer;
rawPacketLen = length;
@ -2389,7 +2419,8 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
internalState = InternalState::DONE;
}
break;
case startracker::Program::FIRMWARE:
case startracker::Program::FIRMWARE_BACKUP:
case startracker::Program::FIRMWARE_MAIN: {
if (startupState == StartupState::WAIT_CHECK_PROGRAM) {
startupState = StartupState::BOOT_BOOTLOADER;
}
@ -2400,9 +2431,10 @@ ReturnValue_t StarTrackerHandler::checkProgram() {
internalState = InternalState::FAILED_BOOTLOADER_BOOT;
}
break;
}
default:
sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID"
<< std::endl;
sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID "
<< static_cast<int>(versionSet.program.value) << std::endl;
return INVALID_PROGRAM;
}
return returnvalue::OK;
@ -2865,14 +2897,15 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
case startracker::REQ_CENTROID:
case startracker::REQ_CENTROIDS:
case startracker::REQ_CONTRAST: {
if (getMode() == MODE_ON and getSubmode() != startracker::Program::FIRMWARE) {
if (getMode() == MODE_ON and getSubmode() != startracker::SUBMODE_FIRMWARE) {
return STARTRACKER_NOT_RUNNING_FIRMWARE;
}
break;
}
case startracker::FIRMWARE_UPDATE:
case startracker::FIRMWARE_UPDATE_MAIN:
case startracker::FIRMWARE_UPDATE_BACKUP:
case startracker::FLASH_READ:
if (getMode() != MODE_ON or getSubmode() != startracker::Program::BOOTLOADER) {
if (getMode() != MODE_ON or getSubmode() != startracker::SUBMODE_BOOTLOADER) {
return STARTRACKER_NOT_RUNNING_BOOTLOADER;
}
break;
@ -2883,3 +2916,42 @@ ReturnValue_t StarTrackerHandler::checkCommand(ActionId_t actionId) {
}
ReturnValue_t StarTrackerHandler::acceptExternalDeviceCommands() { return returnvalue::OK; }
ReturnValue_t StarTrackerHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues,
uint16_t startAtIndex) {
auto firmwareTargetUpdate = [&](bool persistent) {
uint8_t value = 0;
newValues->getElement(&value);
if (value != static_cast<uint8_t>(startracker::FirmwareTarget::MAIN) &&
value != static_cast<uint8_t>(startracker::FirmwareTarget::BACKUP)) {
return HasParametersIF::INVALID_VALUE;
}
parameterWrapper->set(firmwareTargetRaw);
if (persistent) {
if (sdCardIF.isSdCardUsable(std::nullopt)) {
const char* prefix = sdCardIF.getCurrentMountPrefix();
std::filesystem::path path =
std::filesystem::path(prefix) / startracker::FW_TARGET_CFG_PATH;
std::ofstream of(path, std::ofstream::out | std::ofstream::trunc);
if (value == static_cast<uint8_t>(startracker::FirmwareTarget::MAIN)) {
of << "main\n";
} else {
of << "backup\n";
}
} else {
return HasFileSystemIF::FILESYSTEM_INACTIVE;
}
};
return returnvalue::OK;
};
if (uniqueId == startracker::ParamId::FIRMWARE_TARGET) {
return firmwareTargetUpdate(false);
}
if (uniqueId == startracker::ParamId::FIRMWARE_TARGET_PERSISTENT) {
return firmwareTargetUpdate(true);
}
return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
startAtIndex);
}

18
mission/acs/str/StarTrackerHandler.h
View File

@ -11,10 +11,7 @@
#include <set>
#include <thread>
#include "OBSWConfig.h"
#include "devices/powerSwitcherList.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "fsfw/src/fsfw/serialize/SerializeAdapter.h"
#include "fsfw/timemanager/Countdown.h"
extern "C" {
@ -45,7 +42,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
*/
StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
StrComHandler* strHelper, power::Switch_t powerSwitch,
startracker::SdCardConfigPathGetter& cfgPathGetter);
startracker::SdCardConfigPathGetter& cfgPathGetter, SdCardMountedIF& sdCardIF);
virtual ~StarTrackerHandler();
ReturnValue_t initialize() override;
@ -61,6 +58,9 @@ class StarTrackerHandler : public DeviceHandlerBase {
Submode_t getInitialSubmode() override;
ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
const ParameterWrapper* newValues, uint16_t startAtIndex) override;
protected:
void doStartUp() override;
void doShutDown() override;
@ -161,7 +161,8 @@ class StarTrackerHandler : public DeviceHandlerBase {
// Ping request will reply ping with this ID (data field)
static const uint32_t PING_ID = 0x55;
static const uint32_t BOOT_REGION_ID = 1;
uint8_t firmwareTargetRaw = static_cast<uint8_t>(startracker::FirmwareTarget::MAIN);
static const MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
static const uint32_t MUTEX_TIMEOUT = 20;
static const uint32_t BOOT_TIMEOUT = 1000;
@ -314,12 +315,14 @@ class StarTrackerHandler : public DeviceHandlerBase {
std::set<DeviceCommandId_t> additionalRequestedTm{};
std::set<DeviceCommandId_t>::iterator currentSecondaryTmIter;
SdCardMountedIF& sdCardIF;
startracker::SdCardConfigPathGetter& cfgPathGetter;
/**
* @brief Handles internal state
*/
void handleInternalState();
void loadTargetFirmwareFromPersistentCfg();
/**
* @brief Checks mode for commands requiring MODE_ON of MODE_NORMAL for execution.
@ -380,7 +383,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
* @brief Fills command buffer with data to boot image (works only when star tracker is
* in bootloader mode).
*/
void prepareBootCommand();
void prepareBootCommand(startracker::FirmwareTarget target);
/**
* @brief Fills command buffer with command to get the checksum of a flash part
@ -550,6 +553,9 @@ class StarTrackerHandler : public DeviceHandlerBase {
void bootBootloader();
bool reloadJsonCfgFile();
ReturnValue_t acceptExternalDeviceCommands() override;
ReturnValue_t handleFirmwareUpdateCommand(const uint8_t* data, size_t size,
startracker::FirmwareTarget target);
};
#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */

21
mission/acs/str/strHelpers.h
View File

@ -14,6 +14,12 @@ namespace startracker {
static const Submode_t SUBMODE_BOOTLOADER = 1;
static const Submode_t SUBMODE_FIRMWARE = 2;
enum class FirmwareTarget : uint8_t { MAIN = 1, BACKUP = 10 };
static constexpr char FW_TARGET_CFG_PATH[] = "startracker/fw-target.txt";
enum ParamId : uint32_t { FIRMWARE_TARGET = 1, FIRMWARE_TARGET_PERSISTENT = 2 };
class SdCardConfigPathGetter {
public:
virtual ~SdCardConfigPathGetter() = default;
@ -373,7 +379,7 @@ static const DeviceCommandId_t REQ_DEBUG_CAMERA = 80;
static const DeviceCommandId_t LOGLEVEL = 81;
static const DeviceCommandId_t LOGSUBSCRIPTION = 82;
static const DeviceCommandId_t DEBUG_CAMERA = 83;
static const DeviceCommandId_t FIRMWARE_UPDATE = 84;
static const DeviceCommandId_t FIRMWARE_UPDATE_MAIN = 84;
static const DeviceCommandId_t DISABLE_TIMESTAMP_GENERATION = 85;
static const DeviceCommandId_t ENABLE_TIMESTAMP_GENERATION = 86;
static constexpr DeviceCommandId_t SET_TIME_FROM_SYS_TIME = 87;
@ -388,6 +394,7 @@ static constexpr DeviceCommandId_t ADD_SECONDARY_TM_TO_NORMAL_MODE = 95;
static constexpr DeviceCommandId_t RESET_SECONDARY_TM_SET = 96;
static constexpr DeviceCommandId_t READ_SECONDARY_TM_SET = 97;
static constexpr DeviceCommandId_t RELOAD_JSON_CFG_FILE = 100;
static const DeviceCommandId_t FIRMWARE_UPDATE_BACKUP = 101;
static const DeviceCommandId_t NONE = 0xFFFFFFFF;
static const uint32_t VERSION_SET_ID = REQ_VERSION;
@ -489,7 +496,8 @@ static constexpr uint8_t MATCHED_CENTROIDS = 40;
namespace Program {
static const uint8_t BOOTLOADER = 1;
static const uint8_t FIRMWARE = 2;
static const uint8_t FIRMWARE_MAIN = 2;
static const uint8_t FIRMWARE_BACKUP = 3;
} // namespace Program
namespace region_secrets {
@ -509,7 +517,7 @@ static const uint32_t REGION_12_SECRET = 0x42fedef6;
static const uint32_t REGION_13_SECRET = 0xe53cf10d;
static const uint32_t REGION_14_SECRET = 0xe862b70b;
static const uint32_t REGION_15_SECRET = 0x79b537ca;
static const uint32_t secret[16]{
static const uint32_t SECRETS[16]{
REGION_0_SECRET, REGION_1_SECRET, REGION_2_SECRET, REGION_3_SECRET,
REGION_4_SECRET, REGION_5_SECRET, REGION_6_SECRET, REGION_7_SECRET,
REGION_8_SECRET, REGION_9_SECRET, REGION_10_SECRET, REGION_11_SECRET,
@ -538,7 +546,12 @@ enum class FlashSections : uint8_t {
};
// Flash region IDs of firmware partition
enum class FirmwareRegions : uint32_t { FIRST = 1, LAST = 8 };
enum class FirmwareRegions : uint32_t {
FIRST_MAIN = 1,
LAST_MAIN = 8,
FIRST_BACKUP = 10,
LAST_BACKUP = 16
};
static const uint32_t FLASH_REGION_SIZE = 0x20000;

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

@ -717,22 +717,22 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
case (0x11): // KalmanFilterParameters
switch (parameterId) {
case 0x0:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
parameterWrapper->set(kalmanFilterParameters.sensorNoiseStr);
break;
case 0x1:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSS);
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSus);
break;
case 0x2:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMAG);
parameterWrapper->set(kalmanFilterParameters.sensorNoiseMgm);
break;
case 0x3:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGYR);
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyr);
break;
case 0x4:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseArwGYR);
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrArw);
break;
case 0x5:
parameterWrapper->set(kalmanFilterParameters.sensorNoiseBsGYR);
parameterWrapper->set(kalmanFilterParameters.sensorNoiseGyrBs);
break;
default:
return INVALID_IDENTIFIER_ID;

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

@ -8,6 +8,9 @@
typedef unsigned char uint8_t;
class AcsParameters : public HasParametersIF {
private:
static constexpr double DEG2RAD = M_PI / 180.;
public:
AcsParameters();
virtual ~AcsParameters();
@ -22,7 +25,7 @@ class AcsParameters : public HasParametersIF {
uint8_t fusedRateSafeDuringEclipse = true;
uint8_t fusedRateFromStr = true;
uint8_t fusedRateFromQuest = true;
double questFilterWeight = 0.0;
double questFilterWeight = 0.9;
} onBoardParams;
struct InertiaEIVE {
@ -773,7 +776,7 @@ class AcsParameters : public HasParametersIF {
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
-0.000889232196185857, -0.00168429567131815}};
float susBrightnessThreshold = 0.7;
float susVectorFilterWeight = .85;
float susVectorFilterWeight = .95;
float susRateFilterWeight = .99;
} susHandlingParameters;
@ -854,7 +857,7 @@ class AcsParameters : public HasParametersIF {
struct PointingLawParameters {
double zeta = 0.3;
double om = 0.3;
double omMax = 1 * M_PI / 180;
double omMax = 1 * DEG2RAD;
double qiMin = 0.1;
double gainNullspace = 0.01;
@ -876,15 +879,15 @@ class AcsParameters : public HasParametersIF {
uint8_t timeElapsedMax = 10; // rot rate calculations
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeTgt = 500; // [m]
double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude
double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude
double altitudeTgt = 500; // [m]
// For one-axis control:
uint8_t avoidBlindStr = true;
double blindAvoidStart = 1.5;
double blindAvoidStop = 2.5;
double blindRotRate = 1 * M_PI / 180;
double blindRotRate = 1. * DEG2RAD;
} targetModeControllerParameters;
struct GsTargetModeControllerParameters : PointingLawParameters {
@ -892,9 +895,9 @@ class AcsParameters : public HasParametersIF {
uint8_t timeElapsedMax = 10; // rot rate calculations
// Default is Stuttgart GS
double latitudeTgt = 48.7495 * M_PI / 180.; // [rad] Latitude
double longitudeTgt = 9.10384 * M_PI / 180.; // [rad] Longitude
double altitudeTgt = 500; // [m]
double latitudeTgt = 48.7495 * DEG2RAD; // [rad] Latitude
double longitudeTgt = 9.10384 * DEG2RAD; // [rad] Longitude
double altitudeTgt = 500; // [m]
} gsTargetModeControllerParameters;
struct NadirModeControllerParameters : PointingLawParameters {
@ -911,8 +914,8 @@ class AcsParameters : public HasParametersIF {
} inertialModeControllerParameters;
struct StrParameters {
double exclusionAngle = 20 * M_PI / 180;
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // geometry frame
double exclusionAngle = 20. * DEG2RAD;
double boresightAxis[3] = {0.7593, 0.0000, -0.6508}; // body rf
} strParameters;
struct GpsParameters {
@ -925,25 +928,25 @@ class AcsParameters : public HasParametersIF {
struct SunModelParameters {
float domega = 36000.771;
float omega_0 = 280.46 * M_PI / 180.; // RAAN plus argument of
// perigee
float m_0 = 357.5277; // coefficients for mean anomaly
float dm = 35999.049; // coefficients for mean anomaly
float e = 23.4392911 * M_PI / 180.; // angle of earth's rotation axis
float e1 = 0.74508 * M_PI / 180.;
float omega_0 = 280.46 * DEG2RAD; // RAAN plus argument of
// perigee
float m_0 = 357.5277; // coefficients for mean anomaly
float dm = 35999.049; // coefficients for mean anomaly
float e = 23.4392911 * DEG2RAD; // angle of earth's rotation axis
float e1 = 0.74508 * DEG2RAD;
float p1 = 6892. / 3600. * M_PI / 180.; // some parameter
float p2 = 72. / 3600. * M_PI / 180.; // some parameter
float p1 = 6892. / 3600. * DEG2RAD; // some parameter
float p2 = 72. / 3600. * DEG2RAD; // some parameter
} sunModelParameters;
struct KalmanFilterParameters {
double sensorNoiseSTR = 0.1 * M_PI / 180;
double sensorNoiseSS = 8 * M_PI / 180;
double sensorNoiseMAG = 4 * M_PI / 180;
double sensorNoiseGYR = 0.1 * M_PI / 180;
double sensorNoiseStr = 0.1 * DEG2RAD;
double sensorNoiseSus = 8. * DEG2RAD;
double sensorNoiseMgm = 4. * DEG2RAD;
double sensorNoiseGyr = 0.1 * DEG2RAD;
double sensorNoiseArwGYR = 3 * 0.0043 * M_PI / sqrt(10) / 180; // Angular Random Walk
double sensorNoiseBsGYR = 3 * M_PI / 180 / 3600; // Bias Stability
double sensorNoiseGyrArw = 3. * 0.0043 / sqrt(10) * DEG2RAD; // Angular Random Walk
double sensorNoiseGyrBs = 3. / 3600. * DEG2RAD; // Bias Stability
} kalmanFilterParameters;
struct MagnetorquerParameter {
@ -959,8 +962,8 @@ class AcsParameters : public HasParametersIF {
struct DetumbleParameter {
uint8_t detumblecounter = 75; // 30 s
double omegaDetumbleStart = 2 * M_PI / 180;
double omegaDetumbleEnd = 1 * M_PI / 180;
double omegaDetumbleStart = 2 * DEG2RAD;
double omegaDetumbleEnd = 1 * DEG2RAD;
double gainBdot = pow(10.0, -3.3);
double gainFull = pow(10.0, -2.3);
uint8_t useFullDetumbleLaw = false;

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

@ -41,8 +41,8 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
// Sensor Weights
double kSus = 0, kMgm = 0;
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSS, -2);
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMAG, -2);
kSus = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseSus, -2);
kMgm = std::pow(acsParameters->kalmanFilterParameters.sensorNoiseMgm, -2);
// Weighted Vectors
double weightedSusB[3] = {0, 0, 0}, weightedMgmB[3] = {0, 0, 0}, kSusVec[3] = {0, 0, 0},

242
mission/controller/acs/Guidance.cpp
View File

@ -8,8 +8,8 @@ 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
double zAxisXI[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirI, zAxisXI, 3);
double zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::normalize(sunDirI, zAxisIX, 3);
// 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};
@ -17,39 +17,37 @@ void Guidance::targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta,
VectorOperations<double>::normalize(posSatI, helperXI, 3);
// construct y-axis from helper vector and z-axis
double yAxisXI[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisXI, helperXI, yAxisXI);
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisIX, helperXI, yAxisIX);
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
// x-axis completes RHS
double xAxisXI[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxisXI, zAxisXI, xAxisXI);
VectorOperations<double>::normalize(xAxisXI, xAxisXI, 3);
double xAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(yAxisIX, zAxisIX, xAxisIX);
VectorOperations<double>::normalize(xAxisIX, xAxisIX, 3);
// join transformation matrix
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
QuaternionOperations::fromDcm(dcmXI, targetQuat);
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);
// calculate of reference rotation rate
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double velSatF[3], double targetQuat[4],
double targetSatRotRate[3]) {
void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
const double posSatF[3], const double velSatF[3],
double targetQuat[4], double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for target pointing
//-------------------------------------------------------------------------------------
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double targetF[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
CoordinateTransformations::cartesianFromLatLongAlt(
acsParameters->targetModeControllerParameters.latitudeTgt,
acsParameters->targetModeControllerParameters.longitudeTgt,
acsParameters->targetModeControllerParameters.altitudeTgt, targetF);
double targetDirF[3] = {0, 0, 0};
VectorOperations<double>::subtract(targetF, posSatF, targetDirF, 3);
// target direction in the ECI frame
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
@ -59,8 +57,8 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
// x-axis aligned with target direction
// this aligns with the camera, E- and S-band antennas
double xAxisXI[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirI, xAxisXI, 3);
double xAxisIX[3] = {0, 0, 0};
VectorOperations<double>::normalize(targetDirI, xAxisIX, 3);
// transform velocity into inertial frame
double velSatI[3] = {0, 0, 0};
@ -73,32 +71,32 @@ void Guidance::targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta,
// y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
// resolution
double yAxisXI[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalI, xAxisXI, yAxisXI);
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(orbitalNormalI, xAxisIX, yAxisIX);
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
// z-axis completes RHS
double zAxisXI[3] = {0, 0, 0};
VectorOperations<double>::cross(xAxisXI, yAxisXI, zAxisXI);
double zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(xAxisIX, yAxisIX, zAxisIX);
// join transformation matrix
double dcmIX[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
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);
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
}
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double sunDirI[3], double targetQuat[4],
double targetSatRotRate[3]) {
void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta,
const double posSatF[3], const double sunDirI[3],
double targetQuat[4], double targetSatRotRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for ground station pointing
//-------------------------------------------------------------------------------------
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
double posGroundStationF[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(
CoordinateTransformations::cartesianFromLatLongAlt(
acsParameters->gsTargetModeControllerParameters.latitudeTgt,
acsParameters->gsTargetModeControllerParameters.longitudeTgt,
acsParameters->gsTargetModeControllerParameters.altitudeTgt, posGroundStationF);
@ -106,43 +104,93 @@ void Guidance::targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, dou
// target direction in the ECI frame
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);
CoordinateTransformations::positionEcfToEci(posGroundStationF, posGroundStationI, &timeAbsolute);
VectorOperations<double>::subtract(posGroundStationI, posSatI, groundStationDirI, 3);
// negative x-axis aligned with target direction
// this aligns with the camera, E- and S-band antennas
double xAxisXI[3] = {0, 0, 0};
VectorOperations<double>::normalize(groundStationDirI, xAxisXI, 3);
VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
double xAxisIX[3] = {0, 0, 0};
VectorOperations<double>::normalize(groundStationDirI, xAxisIX, 3);
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
// get sun vector model in ECI
VectorOperations<double>::normalize(sunDirI, sunDirI, 3);
// get earth vector in ECI
double earthDirI[3] = {0, 0, 0};
VectorOperations<double>::normalize(posSatI, earthDirI, 3);
VectorOperations<double>::mulScalar(earthDirI, -1, earthDirI, 3);
// calculate z-axis as projection of sun vector into plane defined by x-axis as normal vector
// z = sPerpenticular = s - sParallel = s - (x*s)/norm(x)^2 * x
double xDotS = VectorOperations<double>::dot(xAxisXI, sunDirI);
xDotS /= pow(VectorOperations<double>::norm(xAxisXI, 3), 2);
double sunParallel[3], zAxisXI[3];
VectorOperations<double>::mulScalar(xAxisXI, xDotS, sunParallel, 3);
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxisXI, 3);
VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
// sun avoidance calculations
double sunPerpendicularX[3] = {0, 0, 0}, sunFloorYZ[3] = {0, 0, 0}, zAxisSun[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, sunDirI),
sunPerpendicularX, 3);
VectorOperations<double>::subtract(sunDirI, sunPerpendicularX, sunFloorYZ, 3);
VectorOperations<double>::normalize(sunFloorYZ, sunFloorYZ, 3);
VectorOperations<double>::mulScalar(sunFloorYZ, -1, zAxisSun, 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(zAxisSun, acsParameters->strParameters.boresightAxis[2],
strVecSunZ, 3);
VectorOperations<double>::add(strVecSunX, strVecSunZ, strVecSun, 3);
VectorOperations<double>::normalize(strVecSun, strVecSun, 3);
sunWeight = VectorOperations<double>::dot(strVecSun, sunDirI);
// y-axis completes RHS
double yAxisXI[3];
VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
VectorOperations<double>::normalize(yAxisXI, yAxisXI, 3);
// earth avoidance calculations
double earthPerpendicularX[3] = {0, 0, 0}, earthFloorYZ[3] = {0, 0, 0}, zAxisEarth[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(xAxisIX, VectorOperations<double>::dot(xAxisIX, earthDirI),
earthPerpendicularX, 3);
VectorOperations<double>::subtract(earthDirI, earthPerpendicularX, earthFloorYZ, 3);
VectorOperations<double>::normalize(earthFloorYZ, earthFloorYZ, 3);
VectorOperations<double>::mulScalar(earthFloorYZ, -1, zAxisEarth, 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(zAxisEarth, acsParameters->strParameters.boresightAxis[2],
strVecEarthZ, 3);
VectorOperations<double>::add(strVecEarthX, strVecEarthZ, strVecEarth, 3);
VectorOperations<double>::normalize(strVecEarth, strVecEarth, 3);
earthWeight = VectorOperations<double>::dot(strVecEarth, earthDirI);
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::abs(sunWeight) + std::abs(earthWeight));
sunWeight *= normFactor;
earthWeight *= normFactor;
// calculate z-axis for str blinding avoidance
double zAxisIX[3] = {0, 0, 0};
VectorOperations<double>::mulScalar(zAxisSun, sunWeight, zAxisSun, 3);
VectorOperations<double>::mulScalar(zAxisEarth, earthWeight, zAxisEarth, 3);
VectorOperations<double>::add(zAxisSun, zAxisEarth, zAxisIX, 3);
VectorOperations<double>::mulScalar(zAxisIX, -1, zAxisIX, 3);
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
// calculate y-axis
double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
VectorOperations<double>::normalize(yAxisIX, yAxisIX, 3);
// join transformation matrix
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
QuaternionOperations::fromDcm(dcmXI, targetQuat);
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);
limitReferenceRotation(xAxisIX, targetQuat);
targetRotationRate(timeDelta, targetQuat, targetSatRotRate);
std::memcpy(xAxisIXprev, xAxisIX, sizeof(xAxisIXprev));
}
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatE[3],
double velSatE[3], double targetQuat[4], double refSatRate[3]) {
void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
const double posSatE[3], const double velSatE[3],
double targetQuat[4], double refSatRate[3]) {
//-------------------------------------------------------------------------------------
// Calculation of target quaternion for Nadir pointing
//-------------------------------------------------------------------------------------
@ -153,26 +201,26 @@ void Guidance::targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta,
// negative x-axis aligned with position vector
// this aligns with the camera, E- and S-band antennas
double xAxisXI[3] = {0, 0, 0};
VectorOperations<double>::normalize(posSatI, xAxisXI, 3);
VectorOperations<double>::mulScalar(xAxisXI, -1, xAxisXI, 3);
double xAxisIX[3] = {0, 0, 0};
VectorOperations<double>::normalize(posSatI, xAxisIX, 3);
VectorOperations<double>::mulScalar(xAxisIX, -1, xAxisIX, 3);
// make z-Axis parallel to major part of camera resolution
double zAxisXI[3] = {0, 0, 0};
double zAxisIX[3] = {0, 0, 0};
double velSatI[3] = {0, 0, 0};
CoordinateTransformations::velocityEcfToEci(velSatE, posSatE, velSatI, &timeAbsolute);
VectorOperations<double>::cross(xAxisXI, velSatI, zAxisXI);
VectorOperations<double>::normalize(zAxisXI, zAxisXI, 3);
VectorOperations<double>::cross(xAxisIX, velSatI, zAxisIX);
VectorOperations<double>::normalize(zAxisIX, zAxisIX, 3);
// y-Axis completes RHS
double yAxisXI[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisXI, xAxisXI, yAxisXI);
double yAxisIX[3] = {0, 0, 0};
VectorOperations<double>::cross(zAxisIX, xAxisIX, yAxisIX);
// join transformation matrix
double dcmXI[3][3] = {{xAxisXI[0], yAxisXI[0], zAxisXI[0]},
{xAxisXI[1], yAxisXI[1], zAxisXI[1]},
{xAxisXI[2], yAxisXI[2], zAxisXI[2]}};
QuaternionOperations::fromDcm(dcmXI, targetQuat);
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);
targetRotationRate(timeDelta, targetQuat, refSatRate);
}
@ -189,6 +237,59 @@ void Guidance::targetRotationRate(const double timeDelta, double quatIX[4], doub
std::memcpy(quatIXprev, quatIX, sizeof(quatIXprev));
}
void Guidance::limitReferenceRotation(const double xAxisIX[3], double quatIX[4]) {
if ((VectorOperations<double>::norm(quatIXprev, 4) == 0) or
(VectorOperations<double>::norm(xAxisIXprev, 3) == 0)) {
return;
}
// check required rotation and return if below limit
double quatXprevX[4] = {0, 0, 0, 0}, quatXprevI[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatIXprev, quatXprevI);
QuaternionOperations::multiply(quatIX, quatXprevI, quatXprevX);
QuaternionOperations::normalize(quatXprevX);
double phiMax = acsParameters->gsTargetModeControllerParameters.omMax *
acsParameters->onBoardParams.sampleTime;
if (2 * std::acos(quatXprevX[3]) < phiMax) {
return;
}
// x-axis always needs full rotation
double phiX = 0, phiXvec[3] = {0, 0, 0};
phiX = std::acos(VectorOperations<double>::dot(xAxisIXprev, xAxisIX));
VectorOperations<double>::cross(xAxisIXprev, xAxisIX, phiXvec);
VectorOperations<double>::normalize(phiXvec, phiXvec, 3);
double quatXprevXtilde[4] = {0, 0, 0, 0}, quatIXtilde[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(phiXvec, -std::sin(phiX / 2.), phiXvec, 3);
std::memcpy(quatXprevXtilde, phiXvec, sizeof(phiXvec));
quatXprevXtilde[3] = cos(phiX / 2.);
QuaternionOperations::normalize(quatXprevXtilde);
QuaternionOperations::multiply(quatXprevXtilde, quatIXprev, quatIXtilde);
// use the residual rotation up to the maximum
double quatXXtilde[4] = {0, 0, 0, 0}, quatXI[4] = {0, 0, 0, 0};
QuaternionOperations::inverse(quatIX, quatXI);
QuaternionOperations::multiply(quatIXtilde, quatXI, quatXXtilde);
double phiResidual = 0, phiResidualVec[3] = {0, 0, 0};
phiResidual = std::sqrt((phiMax * phiMax) - (phiX * phiX));
std::memcpy(phiResidualVec, quatXXtilde, sizeof(phiResidualVec));
VectorOperations<double>::normalize(phiResidualVec, phiResidualVec, 3);
double quatXhatXTilde[4] = {0, 0, 0, 0}, quatXTildeXhat[4] = {0, 0, 0, 0};
VectorOperations<double>::mulScalar(phiResidualVec, std::sin(phiResidual / 2.), phiResidualVec,
3);
std::memcpy(quatXhatXTilde, phiResidualVec, sizeof(phiResidualVec));
quatXhatXTilde[3] = std::cos(phiResidual / 2.);
QuaternionOperations::normalize(quatXhatXTilde);
// calculate final quaternion
QuaternionOperations::inverse(quatXhatXTilde, quatXTildeXhat);
QuaternionOperations::multiply(quatXTildeXhat, quatIXtilde, quatIX);
QuaternionOperations::normalize(quatIX);
}
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) {
@ -255,7 +356,10 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
return acsctrl::MULTIPLE_RW_UNAVAILABLE;
}
void Guidance::resetValues() { std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev)); }
void Guidance::resetValues() {
std::memcpy(quatIXprev, ZERO_VEC4, sizeof(quatIXprev));
std::memcpy(xAxisIXprev, ZERO_VEC3, sizeof(xAxisIXprev));
}
void Guidance::getTargetParamsSafe(double sunTargetSafe[3]) {
std::error_code e;

17
mission/controller/acs/Guidance.h
View File

@ -8,9 +8,7 @@
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <mission/controller/acs/AcsParameters.h>
#include <mission/controller/acs/SensorValues.h>
#include <mission/controller/acs/util/MathOperations.h>
#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
#include <time.h>
#include <cmath>
#include <filesystem>
@ -27,16 +25,18 @@ class Guidance {
void targetQuatPtgIdle(timeval timeAbsolute, const double timeDelta, const double sunDirI[3],
const double posSatF[4], double targetQuat[4], double targetSatRotRate[3]);
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, double posSatF[3],
double velSatF[3], double targetQuat[4], double refSatRate[3]);
void targetQuatPtgTarget(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
const double velSatE[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgGs(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
const double sunDirI[3], double quatIX[4], double targetSatRotRate[3]);
void targetQuatPtgNadir(timeval timeAbsolute, const double timeDelta, const double posSatF[3],
const double velSatF[3], double targetQuat[4], double refSatRate[3]);
void targetRotationRate(const double timeDelta, double quatInertialTarget[4],
double *targetSatRotRate);
void limitReferenceRotation(const double xAxisIX[3], double quatIX[4]);
void 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);
@ -54,6 +54,7 @@ class Guidance {
bool strBlindAvoidFlag = false;
double quatIXprev[4] = {0, 0, 0, 0};
double xAxisIXprev[3] = {0, 0, 0};
static constexpr char SD_0_SKEWED_PTG_FILE[] = "/mnt/sd0/conf/acsDeploymentConfirm";
static constexpr char SD_1_SKEWED_PTG_FILE[] = "/mnt/sd1/conf/acsDeploymentConfirm";

22
mission/controller/acs/Igrf13Model.cpp
View File

@ -1,19 +1,5 @@
#include "Igrf13Model.h"
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <stdint.h>
#include <string.h>
#include <time.h>
#include <cmath>
#include "util/MathOperations.h"
using namespace Math;
Igrf13Model::Igrf13Model() {}
Igrf13Model::~Igrf13Model() {}
@ -23,7 +9,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
double magFieldModel[3] = {0, 0, 0};
double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/
theta -= 90 * PI / 180;
theta -= 90. * M_PI / 180.;
theta *= (-1);
double rE = 6371200.0; // radius earth [m]
@ -83,13 +69,13 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModel[1] *= -1;
magFieldModel[2] *= (-1 / sin(theta));
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement);
double UT1 = JD2000 / 36525.;
double gst =
280.46061837 + 360.98564736629 * JD2000 + 0.0003875 * pow(UT1, 2) - 2.6e-8 * pow(UT1, 3);
gst = std::fmod(gst, 360.);
gst *= PI / 180.;
gst *= M_PI / 180.;
double lst = gst + longitude; // local sidereal time [rad]
magFieldModelInertial[0] =
@ -107,7 +93,7 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {
double JD2000Igrf = (2458850.0 - 2451545); // Begin of IGRF-13 (2020-01-01,00:00:00) in JD2000
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeOfMagMeasurement);
double JD2000 = TimeSystems::convertUnixToJD2000(timeOfMagMeasurement);
double days = ceil(JD2000 - JD2000Igrf);
for (int i = 0; i <= igrfOrder; i++) {
for (int j = 0; j <= (igrfOrder - 1); j++) {

9
mission/controller/acs/Igrf13Model.h
View File

@ -16,10 +16,11 @@
#ifndef IGRF13MODEL_H_
#define IGRF13MODEL_H_
#include <fsfw/parameters/HasParametersIF.h>
#include <stdint.h>
#include <string.h>
#include <time.h>
#include <fsfw/src/fsfw/globalfunctions/TimeSystems.h>
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <cmath>

36
mission/controller/acs/MultiplicativeKalmanFilter.cpp
View File

@ -9,9 +9,6 @@
#include <cmath>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter() {}
MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}
@ -25,9 +22,9 @@ ReturnValue_t MultiplicativeKalmanFilter::init(
if (validMagField_ && validSS && validSSModel && validMagModel) {
// QUEST ALGO -----------------------------------------------------------------------
double sigmaSun = 0, sigmaMag = 0, sigmaGyro = 0;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGYR;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
sigmaGyro = acsParameters->kalmanFilterParameters.sensorNoiseGyr;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -234,9 +231,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// If we are here, MEKF will perform
double sigmaSun = 0, sigmaMag = 0, sigmaStr = 0;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSS;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMAG;
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseSTR;
sigmaSun = acsParameters->kalmanFilterParameters.sensorNoiseSus;
sigmaMag = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr;
double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagJ[3] = {0, 0, 0},
normSunJ[3] = {0, 0, 0};
@ -264,8 +261,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
double measSensMatrix11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // ss
double measSensMatrix22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; // mag
double measSensMatrix33[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; // str
MathOperations<double>::skewMatrix(sunEstB, *measSensMatrix11);
MathOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
MatrixOperations<double>::skewMatrix(sunEstB, *measSensMatrix11);
MatrixOperations<double>::skewMatrix(magEstB, *measSensMatrix22);
double measVecQuat[3] = {0, 0, 0};
if (validSTR_) {
@ -837,8 +834,9 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::add(*residualCov, *measCovMatrix, *residualCov, MDF, MDF);
// <<INVERSE residualCov HIER>>
double invResidualCov[MDF][MDF] = {{0}};
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
if (inversionFailed) {
ReturnValue_t result =
MatrixOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
if (result != returnvalue::OK) {
updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
return MEKF_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
}
@ -874,7 +872,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
// State Vector Elements
double xi1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
xi2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(propagatedQuaternion, *xi2);
MatrixOperations<double>::skewMatrix(propagatedQuaternion, *xi2);
double identityMatrix3[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
MatrixOperations<double>::multiplyScalar(*identityMatrix3, propagatedQuaternion[3], *xi1, 3, 3);
MatrixOperations<double>::add(*xi1, *xi2, *xi1, 3, 3);
@ -898,8 +896,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
biasGYR[2] = updatedGyroBias[2];
/* ----------- PROPAGATION ----------*/
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseBsGYR;
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseArwGYR;
double sigmaU = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs;
double sigmaV = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw;
double discTimeMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
{0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
@ -1057,7 +1055,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
VectorOperations<double>::mulScalar(rotRateEst, sinFac, rotSin, 3);
double skewSin[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MathOperations<double>::skewMatrix(rotSin, *skewSin);
MatrixOperations<double>::skewMatrix(rotSin, *skewSin);
MatrixOperations<double>::multiplyScalar(*identityMatrix3, rotCos, *rotCosMat, 3, 3);
double subMatUL[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
@ -1080,8 +1078,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
if (not(MathOperations<double>::checkVectorIsFinite(propagatedQuaternion, 4)) ||
not(MathOperations<double>::checkMatrixIsFinite(initialQuaternion, 6, 6))) {
if (not(VectorOperations<double>::isFinite(propagatedQuaternion, 4)) ||
not(MatrixOperations<double>::isFinite(*initialCovarianceMatrix, 6, 6))) {
updateDataSetWithoutData(mekfData, MekfStatus::NOT_FINITE);
return MEKF_NOT_FINITE;
}

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

@ -5,9 +5,6 @@
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>
#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"
Navigation::Navigation() {}
Navigation::~Navigation() {}

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

@ -180,7 +180,7 @@ void SensorProcessing::processSus(
const AcsParameters::SunModelParameters *sunModelParameters,
acsctrl::SusDataProcessed *susDataProcessed) {
/* -------- Sun Model Direction (IJK frame) ------- */
double JD2000 = MathOperations<double>::convertUnixToJD2000(timeAbsolute);
double JD2000 = TimeSystems::convertUnixToJD2000(timeAbsolute);
// Julean Centuries
double sunIjkModel[3] = {0.0, 0.0, 0.0};
@ -198,6 +198,7 @@ void SensorProcessing::processSus(
sunIjkModel[0] = cos(eclipticLongitude);
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
VectorOperations<double>::normalize(sunIjkModel, sunIjkModel, 3);
uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
if (sus0valid) {
@ -528,8 +529,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
uint8_t gpsSource = acs::gps::Source::NONE;
// We do not trust the GPS and therefore it shall die here if SPG4 is running
if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) {
MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
gdLongitude, altitude);
CoordinateTransformations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value,
gdLatitude, gdLongitude, altitude);
double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
gcLatitude = atan(factor * tan(gdLatitude));
{
@ -559,7 +560,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
// Calculation of the satellite velocity in earth fixed frame
double deltaDistance[3] = {0, 0, 0};
MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
CoordinateTransformations::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
if (validSavedPosSatE and timeDelta < (gpsParameters->timeDiffVelocityMax) and timeDelta > 0) {
VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDelta, gpsVelocityE, 3);

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

@ -2,7 +2,9 @@
#define SENSORPROCESSING_H_
#include <common/config/eive/resultClassIds.h>
#include <fsfw/coordinates/CoordinateTransformations.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/globalfunctions/TimeSystems.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
@ -14,7 +16,6 @@
#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 <cmath>

98
mission/controller/acs/util/CholeskyDecomposition.h
View File

@ -1,98 +0,0 @@
/*
* TinyEKF: Extended Kalman Filter for embedded processors
*
* Copyright (C) 2015 Simon D. Levy
*
* MIT License
*/
#ifndef CHOLESKYDECOMPOSITION_H_
#define CHOLESKYDECOMPOSITION_H_
#include <math.h>
// typedef unsigned int uint8_t;
template <typename T1, typename T2 = T1, typename T3 = T2>
class CholeskyDecomposition {
public:
static int invertCholesky(T1 *matrix, T2 *result, T3 *tempMatrix, const uint8_t dimension) {
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
return cholsl(matrix, result, tempMatrix, dimension);
}
private:
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldc1(double *a, double *p, uint8_t n) {
int8_t i, j, k;
double sum;
for (i = 0; i < n; i++) {
for (j = i; j < n; j++) {
sum = a[i * n + j];
for (k = i - 1; k >= 0; k--) {
sum -= a[i * n + k] * a[j * n + k];
}
if (i == j) {
if (sum <= 0) {
return 1; /* error */
}
p[i] = sqrt(sum);
} else {
a[j * n + i] = sum / p[i];
}
}
}
return 0; /* success */
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t choldcsl(double *A, double *a, double *p, uint8_t n) {
uint8_t i, j, k;
double sum;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++) a[i * n + j] = A[i * n + j];
if (choldc1(a, p, n)) return 1;
for (i = 0; i < n; i++) {
a[i * n + i] = 1 / p[i];
for (j = i + 1; j < n; j++) {
sum = 0;
for (k = i; k < j; k++) {
sum -= a[j * n + k] * a[k * n + i];
}
a[j * n + i] = sum / p[j];
}
}
return 0; /* success */
}
// https://github.com/simondlevy/TinyEKF/blob/master/tiny_ekf.c
static uint8_t cholsl(double *A, double *a, double *p, uint8_t n) {
uint8_t i, j, k;
if (choldcsl(A, a, p, n)) return 1;
for (i = 0; i < n; i++) {
for (j = i + 1; j < n; j++) {
a[i * n + j] = 0.0;
}
}
for (i = 0; i < n; i++) {
a[i * n + i] *= a[i * n + i];
for (k = i + 1; k < n; k++) {
a[i * n + i] += a[k * n + i] * a[k * n + i];
}
for (j = i + 1; j < n; j++) {
for (k = j; k < n; k++) {
a[i * n + j] += a[k * n + i] * a[k * n + j];
}
}
}
for (i = 0; i < n; i++) {
for (j = 0; j < i; j++) {
a[i * n + j] = a[j * n + i];
}
}
return 0; /* success */
}
};
#endif /* CONTRIB_MATH_CHOLESKYDECOMPOSITION_H_ */

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

@ -1,465 +0,0 @@
#ifndef MATH_MATHOPERATIONS_H_
#define MATH_MATHOPERATIONS_H_
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/sign.h>
#include <fsfw/src/fsfw/serviceinterface.h>
#include <cmath>
#include <cstring>
#include <iostream>
template <typename T1, typename T2 = T1>
class MathOperations {
public:
static void skewMatrix(const T1 vector[], T2 *result) {
// Input Dimension [3], Output [3][3]
result[0] = 0;
result[1] = -vector[2];
result[2] = vector[1];
result[3] = vector[2];
result[4] = 0;
result[5] = -vector[0];
result[6] = -vector[1];
result[7] = vector[0];
result[8] = 0;
}
static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
uint8_t size = 3) {
// Looks like MatrixOpertions::multiply is able to do the same thing
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
result[resultColumn + size * resultRow] =
vector1[resultRow] * transposeVector2[resultColumn];
}
}
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
matrixMag[i][j] = magEstB[i] * magEstB[j];
matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
}
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
int min_idx;
T1 temp;
std::memcpy(result, matrix, rowSize * colSize * sizeof(*result));
// One by one move boundary of unsorted subarray
for (int k = 0; k < rowSize; k++) {
for (int i = 0; i < colSize - 1; i++) {
// Find the minimum element in unsorted array
min_idx = i;
for (int j = i + 1; j < colSize; j++) {
if (result[j + k * colSize] < result[min_idx + k * colSize]) {
min_idx = j;
}
}
// Swap the found minimum element with the first element
temp = result[i + k * colSize];
result[i + k * colSize] = result[min_idx + k * colSize];
result[min_idx + k * colSize] = temp;
}
}
}
static void convertDateToJD2000(const T1 time, T2 julianDate) {
// time = { Y, M, D, h, m,s}
// time in sec and microsec -> The Epoch (unixtime)
julianDate = 1721013.5 + 367 * time[0] - floor(7 / 4 * (time[0] + (time[1] + 9) / 12)) +
floor(275 * time[1] / 9) + time[2] +
(60 * time[3] + time[4] + (time(5) / 60)) / 1440;
}
static T1 convertUnixToJD2000(timeval time) {
// time = {{s},{us}}
T1 julianDate2000;
julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
return julianDate2000;
}
static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
// convention q = [qx,qy,qz, qw]
outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);
outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
}
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt,
T2 *cartesianOutput) {
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
* longitude and altitude
* @param: lat geodetic latitude [rad]
* longi longitude [rad]
* alt altitude [m]
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
* Landis Markley and John L. Crassidis*/
double radiusPolar = 6356752.314;
double radiusEqua = 6378137;
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
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
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* outputDotDcmEJ Derivative of transformation matrix [3][3]
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
* Landis Markley and John L. Crassidis*/
double JD2000Floor = 0;
double JD2000 = convertUnixToJD2000(time);
// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
JD2000Floor = floor(JD2000);
if ((JD2000 - JD2000Floor) < 0.5) {
JD2000Floor -= 0.5;
} else {
JD2000Floor += 0.5;
}
double JC2000 = JD2000Floor / 36525;
double sec = (JD2000 - JD2000Floor) * 86400;
double gmst = 0; // greenwich mean sidereal time
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
double rest = gmst / 86400;
double FloorRest = floor(rest);
double secOfDay = rest - FloorRest;
secOfDay *= 86400;
gmst = secOfDay / 240 * M_PI / 180;
outputDcmEJ[0] = cos(gmst);
outputDcmEJ[1] = sin(gmst);
outputDcmEJ[2] = 0;
outputDcmEJ[3] = -sin(gmst);
outputDcmEJ[4] = cos(gmst);
outputDcmEJ[5] = 0;
outputDcmEJ[6] = 0;
outputDcmEJ[7] = 0;
outputDcmEJ[8] = 1;
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
{outputDcmEJ[3], outputDcmEJ[4], outputDcmEJ[5]},
{outputDcmEJ[6], outputDcmEJ[7], outputDcmEJ[8]}};
double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = 0.000072921158553;
double dotDcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
}
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
* give also the back the derivative of this matrix
* @param: unixTime Current time in Unix format
* outputDcmEJ Transformation matrix from ECI (J) to ECEF (E) [3][3]
* outputDotDcmEJ Derivative of transformation matrix [3][3]
* @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
* Oliver Zeile
*
https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
static void ecfToEciWithNutPre(timeval unixTime, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
// TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10
//(initial Offset) International Atomic Time (TAI)
double JD2000UTC1 = convertUnixToJD2000(unixTime);
// Julian Date / century from TT
timeval terestrialTime = unixTime;
terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
double JD2000TT = convertUnixToJD2000(terestrialTime);
double JC2000TT = JD2000TT / 36525;
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth rotation Theta
//-------------------------------------------------------------------------------------
double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// Earth Rotation angle
double era = 0;
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 * M_PI / (180 * 3600);
gmst2000 *= arcsecFactor;
gmst2000 += era;
theta[0][0] = cos(gmst2000);
theta[0][1] = sin(gmst2000);
theta[0][2] = 0;
theta[1][0] = -sin(gmst2000);
theta[1][1] = cos(gmst2000);
theta[1][2] = 0;
theta[2][0] = 0;
theta[2][1] = 0;
theta[2][2] = 1;
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Precession P
//-------------------------------------------------------------------------------------
double precession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT, 2) + 0.017998 * pow(JC2000TT, 3);
double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT, 2) - 0.041833 * pow(JC2000TT, 3);
double ze = zeta + 0.79280 * pow(JC2000TT, 2) + 0.000205 * pow(JC2000TT, 3);
zeta *= arcsecFactor;
theta2 *= arcsecFactor;
ze *= arcsecFactor;
precession[0][0] = -sin(ze) * sin(zeta) + cos(ze) * cos(theta2) * cos(zeta);
precession[1][0] = cos(ze) * sin(zeta) + sin(ze) * cos(theta2) * cos(zeta);
precession[2][0] = sin(theta2) * cos(zeta);
precession[0][1] = -sin(ze) * cos(zeta) - cos(ze) * cos(theta2) * sin(zeta);
precession[1][1] = cos(ze) * cos(zeta) - sin(ze) * cos(theta2) * sin(zeta);
precession[2][1] = -sin(theta2) * sin(zeta);
precession[0][2] = -cos(ze) * sin(theta2);
precession[1][2] = -sin(ze) * sin(theta2);
precession[2][2] = cos(theta2);
//-------------------------------------------------------------------------------------
// Calculation of Transformation from earth Nutation N
//-------------------------------------------------------------------------------------
double nutation[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
// lunar asc node
double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
7.455 * pow(JC2000TT, 2) + 0.008 * pow(JC2000TT, 3);
Om *= arcsecFactor;
// delta psi approx
double dp = -17.2 * arcsecFactor * sin(Om);
// delta eps approx
double de = 9.203 * arcsecFactor * cos(Om);
// % true obliquity of the ecliptic eps p.71 (simplified)
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);
nutation[2][0] = sin(e + de) * sin(dp);
nutation[0][1] = -cos(e) * sin(dp);
nutation[1][1] = cos(e) * cos(e + de) * cos(dp) + sin(e) * sin(e + de);
nutation[2][1] = cos(e) * sin(e + de) * cos(dp) - sin(e) * cos(e + de);
nutation[0][2] = -sin(e) * sin(dp);
nutation[1][2] = sin(e) * cos(e + de) * cos(dp) - cos(e) * sin(e + de);
nutation[2][2] = sin(e) * sin(e + de) * cos(dp) + cos(e) * cos(e + de);
//-------------------------------------------------------------------------------------
// Calculation of Derivative of rotation matrix from earth
//-------------------------------------------------------------------------------------
double thetaDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
double dotMatrix[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
double omegaEarth = 0.000072921158553;
MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);
//-------------------------------------------------------------------------------------
// Calculation of transformation matrix and Derivative of transformation matrix
//-------------------------------------------------------------------------------------
double nutationPrecession[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
}
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
int i, j;
double determinant = 0;
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
{matrix[3], matrix[4], matrix[5]},
{matrix[6], matrix[7], matrix[8]}};
for (i = 0; i < 3; i++) {
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
}
// cout<<"\n\ndeterminant: "<<determinant;
// cout<<"\n\nInverse of matrix is: \n";
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
determinant;
}
}
}
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
/* do not use this. takes 300ms */
float det = 0;
T1 matrix[size][size], submatrix[size - 1][size - 1];
for (uint8_t row = 0; row < size; row++) {
for (uint8_t col = 0; col < size; col++) {
matrix[row][col] = inputMatrix[row * size + col];
}
}
if (size == 2)
return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
else {
for (uint8_t col = 0; col < size; col++) {
int subRow = 0;
for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
int subCol = 0;
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
if (colIndex == col) continue;
submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
subCol++;
}
subRow++;
}
det += (pow(-1, col) * matrix[0][col] *
MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
}
}
return det;
}
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
// Stopwatch stopwatch;
T1 matrix[size][size], identity[size][size];
// reformat array to matrix
for (uint8_t row = 0; row < size; row++) {
for (uint8_t col = 0; col < size; col++) {
matrix[row][col] = inputMatrix[row * size + col];
}
}
// init identity matrix
std::memset(identity, 0.0, sizeof(identity));
for (uint8_t diag = 0; diag < size; diag++) {
identity[diag][diag] = 1;
}
// gauss-jordan algo
// sort matrix such as no diag entry shall be 0
for (uint8_t row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
bool swaped = false;
uint8_t rowIndex = 0;
while ((rowIndex < size) && !swaped) {
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
}
swaped = true;
}
rowIndex++;
}
if (!swaped) {
return 1; // matrix not invertible
}
}
}
for (int row = 0; row < size; row++) {
if (matrix[row][row] == 0.0) {
uint8_t rowIndex;
if (row == 0) {
rowIndex = size - 1;
} else {
rowIndex = row - 1;
}
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
}
row--;
if (row < 0) {
return 1; // Matrix is not invertible
}
}
}
// remove non diag elements in matrix (jordan)
for (int row = 0; row < size; row++) {
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
if (row != rowIndex) {
double ratio = matrix[rowIndex][row] / matrix[row][row];
for (int colIndex = 0; colIndex < size; colIndex++) {
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
}
}
}
}
// normalize rows in matrix (gauss)
for (int row = 0; row < size; row++) {
for (int col = 0; col < size; col++) {
identity[row][col] = identity[row][col] / matrix[row][row];
}
}
std::memcpy(inverse, identity, sizeof(identity));
return 0; // successful inversion
}
static bool checkVectorIsFinite(const T1 *inputVector, uint8_t size) {
for (uint8_t i = 0; i < size; i++) {
if (not isfinite(inputVector[i])) {
return false;
}
}
return true;
}
static bool checkMatrixIsFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
for (uint8_t col = 0; col < cols; col++) {
for (uint8_t row = 0; row < rows; row++) {
if (not isfinite(inputMatrix[row * cols + cols])) {
return false;
}
}
}
return true;
}
};
#endif /* ACS_MATH_MATHOPERATIONS_H_ */

26
mission/genericFactory.cpp
View File

@ -250,20 +250,30 @@ void ObjectFactory::produceGenericObjects(HealthTableIF** healthTable_, PusTmFun
pus::PUS_SERVICE_2, 3, 10);
new Service3Housekeeping(objects::PUS_SERVICE_3_HOUSEKEEPING, config::EIVE_PUS_APID,
pus::PUS_SERVICE_3, config::HK_SERVICE_QUEUE_DEPTH, 16);
new Service5EventReporting(
PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5),
80, 160);
auto psbParamsService5 =
PsbParams(objects::PUS_SERVICE_5_EVENT_REPORTING, config::EIVE_PUS_APID, pus::PUS_SERVICE_5);
psbParamsService5.requestQueueDepth = 50;
psbParamsService5.maxPacketsPerCycle = 50;
new Service5EventReporting(psbParamsService5, 80, 160);
new Service8FunctionManagement(objects::PUS_SERVICE_8_FUNCTION_MGMT, config::EIVE_PUS_APID,
pus::PUS_SERVICE_8, config::ACTION_SERVICE_QUEUE_DEPTH, 16, 60);
new Service9TimeManagement(
PsbParams(objects::PUS_SERVICE_9_TIME_MGMT, config::EIVE_PUS_APID, pus::PUS_SERVICE_9));
new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(
PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11),
ccsdsDistrib);
auto psbParamsService11 =
PsbParams(objects::PUS_SERVICE_11_TC_SCHEDULER, config::EIVE_PUS_APID, pus::PUS_SERVICE_11);
psbParamsService11.requestQueueDepth = 100;
psbParamsService11.maxPacketsPerCycle = 100;
new Service11TelecommandScheduling<common::OBSW_MAX_SCHEDULED_TCS>(psbParamsService11,
ccsdsDistrib);
new Service15TmStorage(objects::PUS_SERVICE_15_TM_STORAGE, config::EIVE_PUS_APID, 10);
new Service17Test(
PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17));
auto psbParamsService17 =
PsbParams(objects::PUS_SERVICE_17_TEST, config::EIVE_PUS_APID, pus::PUS_SERVICE_17);
psbParamsService17.requestQueueDepth = 50;
psbParamsService17.maxPacketsPerCycle = 50;
new Service17Test(psbParamsService17);
new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, config::EIVE_PUS_APID,
pus::PUS_SERVICE_20);
new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT, config::EIVE_PUS_APID,

2
tmtc

@ -1 +1 @@
Subproject commit a5ebac626682e86b45f991c919a3ce9a9b7fcfcc
Subproject commit 588d7a8079194c6c51d6ecafd4c940cb1bf0554c