Merge remote-tracking branch 'origin/develop' into thermal_controller
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
This commit is contained in:
commit
d6f537da5f
43
CHANGELOG.md
43
CHANGELOG.md
@ -12,10 +12,51 @@ Starting at v2.0.0, this project will adhere to semantic versioning and the the
|
||||
will consitute of a breaking change warranting a new major release:
|
||||
|
||||
- The TMTC interface changes in any shape of form.
|
||||
- The behavour of the OBSW changes in a major shape or form relevant for operations
|
||||
- The behaviour of the OBSW changes in a major shape or form relevant for operations
|
||||
|
||||
# [unreleased]
|
||||
|
||||
# [v1.31.0]
|
||||
|
||||
eive-tmtc: v2.16.0
|
||||
|
||||
## Fixed
|
||||
|
||||
- Usage of floats as iterators and using them to calculate a uint8_t index in `SusConverter`
|
||||
- Removed unused variables in the `AcsController`
|
||||
- Remove shadowing variables inside ACS assembly classes.
|
||||
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/385
|
||||
|
||||
## Changed
|
||||
|
||||
COM PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/364
|
||||
|
||||
* Moved transmitter timer and handling of carrier and bitlock event from CCSDS handler to COM
|
||||
subsystem
|
||||
* Added parameter command to be able to change the transmitter timeout
|
||||
* Solves [#362](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/362)
|
||||
* Solves [#360](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/360)
|
||||
* Solves [#361](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/361)
|
||||
* Solves [#386](https://egit.irs.uni-stuttgart.de/eive/eive-obsw/issues/386)
|
||||
- All `targetQuat` functions in `Guidance` now return the target quaternion (target
|
||||
in ECI frame), which is passed on to `CtrlValData`.
|
||||
- Moved polling sequence table definitions and source code to `mission/core` folder.
|
||||
PR: https://egit.irs.uni-stuttgart.de/eive/eive-obsw/pulls/395
|
||||
|
||||
## Added
|
||||
|
||||
- `MEKF` now returns an unique returnvalue depending on why the function terminates. These
|
||||
returnvalues are used in the `AcsController` to determine on how to procede with its
|
||||
perform functions. In case the `MEKF` did terminate before estimating the quaternion
|
||||
and rotational rate, an info event will be triggered. Another info event can only be
|
||||
triggered after the `MEKF` has run successfully again. If the `AcsController` tries to
|
||||
perform any pointing mode and the `MEKF` fails, the `performPointingCtrl` function will
|
||||
set the RWs to the last RW speeds and set a zero dipole vector. If the `MEKF` does not
|
||||
recover within 5 cycles (2 mins) the `AcsController` triggers another event, resulting in
|
||||
the `AcsSubsystem` being commanded to `SAFE`.
|
||||
- `MekfData` now includes `mekfStatus`
|
||||
- `CtrlValData` now includes `tgtRotRate`
|
||||
|
||||
# [v1.30.0]
|
||||
|
||||
eive-tmtc: v2.14.0
|
||||
|
||||
@ -10,7 +10,7 @@
|
||||
cmake_minimum_required(VERSION 3.13)
|
||||
|
||||
set(OBSW_VERSION_MAJOR 1)
|
||||
set(OBSW_VERSION_MINOR 30)
|
||||
set(OBSW_VERSION_MINOR 31)
|
||||
set(OBSW_VERSION_REVISION 0)
|
||||
|
||||
# set(CMAKE_VERBOSE TRUE)
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
/**
|
||||
* @brief Auto-generated event translation file. Contains 258 translations.
|
||||
* @brief Auto-generated event translation file. Contains 260 translations.
|
||||
* @details
|
||||
* Generated on: 2023-02-22 15:00:34
|
||||
* Generated on: 2023-02-23 15:39:20
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
@ -93,6 +93,8 @@ const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR";
|
||||
const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
|
||||
const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
|
||||
const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
|
||||
const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
|
||||
const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
|
||||
const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
|
||||
const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
|
||||
const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
|
||||
@ -437,6 +439,10 @@ const char *translateEvents(Event event) {
|
||||
return SAFE_RATE_RECOVERY_STRING;
|
||||
case (11202):
|
||||
return MULTIPLE_RW_INVALID_STRING;
|
||||
case (11203):
|
||||
return MEKF_INVALID_INFO_STRING;
|
||||
case (11204):
|
||||
return MEKF_INVALID_MODE_VIOLATION_STRING;
|
||||
case (11300):
|
||||
return SWITCH_CMD_SENT_STRING;
|
||||
case (11301):
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 148 translations.
|
||||
* Generated on: 2023-02-22 15:00:34
|
||||
* Generated on: 2023-02-23 15:39:20
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
||||
@ -573,7 +573,6 @@ void ObjectFactory::createSyrlinksComponents(PowerSwitchIF* pwrSwitcher) {
|
||||
new SyrlinksHandler(objects::SYRLINKS_HANDLER, objects::UART_COM_IF, syrlinksUartCookie,
|
||||
pcdu::PDU1_CH1_SYRLINKS_12V, syrlinksFdir);
|
||||
syrlinksHandler->setPowerSwitcher(pwrSwitcher);
|
||||
syrlinksHandler->setStartUpImmediately();
|
||||
syrlinksHandler->connectModeTreeParent(satsystem::com::SUBSYSTEM);
|
||||
#if OBSW_DEBUG_SYRLINKS == 1
|
||||
syrlinksHandler->setDebugMode(true);
|
||||
@ -756,15 +755,10 @@ ReturnValue_t ObjectFactory::createCcsdsComponents(LinuxLibgpioIF* gpioComIF,
|
||||
AxiPtmeConfig* axiPtmeConfig =
|
||||
new AxiPtmeConfig(objects::AXI_PTME_CONFIG, q7s::UIO_PTME, q7s::uiomapids::PTME_CONFIG);
|
||||
PtmeConfig* ptmeConfig = new PtmeConfig(objects::PTME_CONFIG, axiPtmeConfig);
|
||||
#if OBSW_ENABLE_SYRLINKS_TRANSMIT_TIMEOUT == 1
|
||||
// Set to high value when not sending via syrlinks
|
||||
static const uint32_t TRANSMITTER_TIMEOUT = 86400000; // 1 day
|
||||
#else
|
||||
static const uint32_t TRANSMITTER_TIMEOUT = 900000; // 15 minutes
|
||||
#endif
|
||||
*ipCoreHandler = new CcsdsIpCoreHandler(
|
||||
objects::CCSDS_HANDLER, objects::PTME, objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig,
|
||||
gpioComIF, gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA, TRANSMITTER_TIMEOUT);
|
||||
|
||||
*ipCoreHandler = new CcsdsIpCoreHandler(objects::CCSDS_HANDLER, objects::PTME,
|
||||
objects::CCSDS_PACKET_DISTRIBUTOR, ptmeConfig, gpioComIF,
|
||||
gpioIds::RS485_EN_TX_CLOCK, gpioIds::RS485_EN_TX_DATA);
|
||||
VirtualChannel* vc = nullptr;
|
||||
vc = new VirtualChannel(ccsds::VC0, config::VC0_QUEUE_SIZE, objects::CCSDS_HANDLER);
|
||||
(*ipCoreHandler)->addVirtualChannel(ccsds::VC0, vc);
|
||||
|
||||
@ -17,10 +17,10 @@
|
||||
#include "fsfw/tasks/FixedTimeslotTaskIF.h"
|
||||
#include "fsfw/tasks/PeriodicTaskIF.h"
|
||||
#include "fsfw/tasks/TaskFactory.h"
|
||||
#include "mission/core/pollingSeqTables.h"
|
||||
#include "mission/core/scheduling.h"
|
||||
#include "mission/devices/devicedefinitions/Max31865Definitions.h"
|
||||
#include "mission/utility/InitMission.h"
|
||||
#include "pollingsequence/pollingSequenceFactory.h"
|
||||
|
||||
/* This is configured for linux without CR */
|
||||
#ifdef PLATFORM_UNIX
|
||||
@ -404,7 +404,7 @@ void scheduling::createPstTasks(TaskFactory& factory, TaskDeadlineMissedFunction
|
||||
#ifdef RELEASE_BUILD
|
||||
static constexpr float acsPstPeriod = 0.4;
|
||||
#else
|
||||
static constexpr float acsPstPeriod = 0.8;
|
||||
static constexpr float acsPstPeriod = 0.4;
|
||||
#endif
|
||||
FixedTimeslotTaskIF* acsTcsPst = factory.createFixedTimeslotTask(
|
||||
"ACS_TCS_PST", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, acsPstPeriod, missedDeadlineFunc);
|
||||
|
||||
@ -1,11 +1,10 @@
|
||||
#ifndef BSP_Q7S_INITMISSION_H_
|
||||
#define BSP_Q7S_INITMISSION_H_
|
||||
|
||||
#include <pollingsequence/pollingSequenceFactory.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "fsfw/tasks/definitions.h"
|
||||
#include "mission/core/pollingSeqTables.h"
|
||||
|
||||
using pst::AcsPstCfg;
|
||||
|
||||
|
||||
@ -62,7 +62,8 @@ static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30;
|
||||
static constexpr uint32_t SCHED_BLOCK_3_READ_IMTQ_MGM_MS = 42;
|
||||
static constexpr uint32_t SCHED_BLOCK_4_ACS_CTRL_MS = 45;
|
||||
static constexpr uint32_t SCHED_BLOCK_5_ACTUATOR_MS = 50;
|
||||
static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 75;
|
||||
static constexpr uint32_t SCHED_BLOCK_6_IMTQ_BLOCK_2_MS = 90;
|
||||
static constexpr uint32_t SCHED_BLOCK_RTD = 150;
|
||||
static constexpr uint32_t SCHED_BLOCK_7_RW_READ_MS = 300;
|
||||
|
||||
// 15 ms for FM
|
||||
@ -76,6 +77,7 @@ static constexpr float SCHED_BLOCK_5_PERIOD = static_cast<float>(SCHED_BLOCK_5_A
|
||||
static constexpr float SCHED_BLOCK_6_PERIOD =
|
||||
static_cast<float>(SCHED_BLOCK_6_IMTQ_BLOCK_2_MS) / 400.0;
|
||||
static constexpr float SCHED_BLOCK_7_PERIOD = static_cast<float>(SCHED_BLOCK_7_RW_READ_MS) / 400.0;
|
||||
static constexpr float SCHED_BLOCK_RTD_PERIOD = static_cast<float>(SCHED_BLOCK_RTD) / 400.0;
|
||||
|
||||
} // namespace acs
|
||||
|
||||
|
||||
@ -87,6 +87,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
||||
11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acsDefs.h
|
||||
11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acsDefs.h
|
||||
11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h
|
||||
11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h
|
||||
11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h
|
||||
11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
|
||||
11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
|
||||
11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h
|
||||
|
||||
|
@ -2,7 +2,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
||||
0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
|
||||
0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
|
||||
0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h
|
||||
0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.h
|
||||
0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
|
||||
0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
|
||||
0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
|
||||
@ -53,9 +52,13 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
||||
0x6a01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
|
||||
0x6b01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
|
||||
0x6c01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
|
||||
0x6901;ACSKAL_KalmanNoGyrMeas;No description;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6902;ACSKAL_KalmanNoModel;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6903;ACSKAL_KalmanInversionFailed;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6902;ACSKAL_KalmanUninitialized;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6903;ACSKAL_KalmanNoGyrData;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6904;ACSKAL_KalmanNoModelVectors;No description;4;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6905;ACSKAL_KalmanNoSusMgmStrData;No description;5;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6906;ACSKAL_KalmanCovarianceInversionFailed;No description;6;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6907;ACSKAL_KalmanInitialized;No description;7;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6908;ACSKAL_KalmanRunning;No description;8;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
|
||||
0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
|
||||
0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
|
||||
|
||||
|
@ -87,6 +87,8 @@ Event ID (dec); Event ID (hex); Name; Severity; Description; File Path
|
||||
11200;0x2bc0;SAFE_RATE_VIOLATION;MEDIUM;No description;mission/acsDefs.h
|
||||
11201;0x2bc1;SAFE_RATE_RECOVERY;MEDIUM;No description;mission/acsDefs.h
|
||||
11202;0x2bc2;MULTIPLE_RW_INVALID;HIGH;No description;mission/acsDefs.h
|
||||
11203;0x2bc3;MEKF_INVALID_INFO;INFO;No description;mission/acsDefs.h
|
||||
11204;0x2bc4;MEKF_INVALID_MODE_VIOLATION;HIGH;No description;mission/acsDefs.h
|
||||
11300;0x2c24;SWITCH_CMD_SENT;INFO;Indicates that a FSFW object requested setting a switch P1: 1 if on was requested, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
|
||||
11301;0x2c25;SWITCH_HAS_CHANGED;INFO;Indicated that a switch state has changed P1: New switch state, 1 for on, 0 for off | P2: Switch Index;mission/devices/devicedefinitions/powerDefinitions.h
|
||||
11302;0x2c26;SWITCHING_Q7S_DENIED;MEDIUM;No description;mission/devices/devicedefinitions/powerDefinitions.h
|
||||
|
||||
|
@ -2,7 +2,6 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
||||
0x0000;OK;System-wide code for ok.;0;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
|
||||
0x0001;Failed;Unspecified system-wide code for failed.;1;HasReturnvaluesIF;fsfw/returnvalues/returnvalue.h
|
||||
0x63a0;NVMB_KeyNotExists;Specified key does not exist in json file;160;NVM_PARAM_BASE;mission/memory/NVMParameterBase.h
|
||||
0x6300;NVMB_Busy;No description;0;NVM_PARAM_BASE;mission/system/objects/Stack5VHandler.h
|
||||
0x5100;IMTQ_InvalidCommandCode;No description;0;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
|
||||
0x5101;IMTQ_MgmMeasurementLowLevelError;No description;1;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
|
||||
0x5102;IMTQ_ActuateCmdLowLevelError;No description;2;IMTQ_HANDLER;mission/devices/devicedefinitions/imtqHelpers.h
|
||||
@ -53,9 +52,13 @@ Full ID (hex); Name; Description; Unique ID; Subsytem Name; File Path
|
||||
0x6a01;ACSSAF_SafectrlMekfInputInvalid;No description;1;ACS_SAFE;mission/controller/acs/control/SafeCtrl.h
|
||||
0x6b01;ACSPTG_PtgctrlMekfInputInvalid;No description;1;ACS_PTG;mission/controller/acs/control/PtgCtrl.h
|
||||
0x6c01;ACSDTB_DetumbleNoSensordata;No description;1;ACS_DETUMBLE;mission/controller/acs/control/Detumble.h
|
||||
0x6901;ACSKAL_KalmanNoGyrMeas;No description;1;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6902;ACSKAL_KalmanNoModel;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6903;ACSKAL_KalmanInversionFailed;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6902;ACSKAL_KalmanUninitialized;No description;2;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6903;ACSKAL_KalmanNoGyrData;No description;3;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6904;ACSKAL_KalmanNoModelVectors;No description;4;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6905;ACSKAL_KalmanNoSusMgmStrData;No description;5;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6906;ACSKAL_KalmanCovarianceInversionFailed;No description;6;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6907;ACSKAL_KalmanInitialized;No description;7;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x6908;ACSKAL_KalmanRunning;No description;8;ACS_KALMAN;mission/controller/acs/MultiplicativeKalmanFilter.h
|
||||
0x4500;HSPI_OpeningFileFailed;No description;0;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
|
||||
0x4501;HSPI_FullDuplexTransferFailed;No description;1;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
|
||||
0x4502;HSPI_HalfDuplexTransferFailed;No description;2;HAL_SPI;fsfw/src/fsfw_hal/linux/spi/SpiComIF.h
|
||||
|
||||
|
@ -1,7 +1,7 @@
|
||||
/**
|
||||
* @brief Auto-generated event translation file. Contains 258 translations.
|
||||
* @brief Auto-generated event translation file. Contains 260 translations.
|
||||
* @details
|
||||
* Generated on: 2023-02-22 15:00:34
|
||||
* Generated on: 2023-02-23 15:39:20
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
@ -93,6 +93,8 @@ const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR";
|
||||
const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
|
||||
const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
|
||||
const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
|
||||
const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
|
||||
const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
|
||||
const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
|
||||
const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
|
||||
const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
|
||||
@ -437,6 +439,10 @@ const char *translateEvents(Event event) {
|
||||
return SAFE_RATE_RECOVERY_STRING;
|
||||
case (11202):
|
||||
return MULTIPLE_RW_INVALID_STRING;
|
||||
case (11203):
|
||||
return MEKF_INVALID_INFO_STRING;
|
||||
case (11204):
|
||||
return MEKF_INVALID_MODE_VIOLATION_STRING;
|
||||
case (11300):
|
||||
return SWITCH_CMD_SENT_STRING;
|
||||
case (11301):
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 153 translations.
|
||||
* Generated on: 2023-02-22 15:00:34
|
||||
* Generated on: 2023-02-23 15:39:20
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
||||
@ -1,5 +1,4 @@
|
||||
target_sources(${OBSW_NAME} PRIVATE ipc/MissionMessageTypes.cpp
|
||||
pollingsequence/pollingSequenceFactory.cpp)
|
||||
target_sources(${OBSW_NAME} PRIVATE ipc/MissionMessageTypes.cpp)
|
||||
|
||||
target_include_directories(${OBSW_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
/**
|
||||
* @brief Auto-generated event translation file. Contains 258 translations.
|
||||
* @brief Auto-generated event translation file. Contains 260 translations.
|
||||
* @details
|
||||
* Generated on: 2023-02-22 15:00:34
|
||||
* Generated on: 2023-02-23 15:39:20
|
||||
*/
|
||||
#include "translateEvents.h"
|
||||
|
||||
@ -93,6 +93,8 @@ const char *SERIALIZATION_ERROR_STRING = "SERIALIZATION_ERROR";
|
||||
const char *SAFE_RATE_VIOLATION_STRING = "SAFE_RATE_VIOLATION";
|
||||
const char *SAFE_RATE_RECOVERY_STRING = "SAFE_RATE_RECOVERY";
|
||||
const char *MULTIPLE_RW_INVALID_STRING = "MULTIPLE_RW_INVALID";
|
||||
const char *MEKF_INVALID_INFO_STRING = "MEKF_INVALID_INFO";
|
||||
const char *MEKF_INVALID_MODE_VIOLATION_STRING = "MEKF_INVALID_MODE_VIOLATION";
|
||||
const char *SWITCH_CMD_SENT_STRING = "SWITCH_CMD_SENT";
|
||||
const char *SWITCH_HAS_CHANGED_STRING = "SWITCH_HAS_CHANGED";
|
||||
const char *SWITCHING_Q7S_DENIED_STRING = "SWITCHING_Q7S_DENIED";
|
||||
@ -437,6 +439,10 @@ const char *translateEvents(Event event) {
|
||||
return SAFE_RATE_RECOVERY_STRING;
|
||||
case (11202):
|
||||
return MULTIPLE_RW_INVALID_STRING;
|
||||
case (11203):
|
||||
return MEKF_INVALID_INFO_STRING;
|
||||
case (11204):
|
||||
return MEKF_INVALID_MODE_VIOLATION_STRING;
|
||||
case (11300):
|
||||
return SWITCH_CMD_SENT_STRING;
|
||||
case (11301):
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
* @brief Auto-generated object translation file.
|
||||
* @details
|
||||
* Contains 153 translations.
|
||||
* Generated on: 2023-02-22 15:00:34
|
||||
* Generated on: 2023-02-23 15:39:20
|
||||
*/
|
||||
#include "translateObjects.h"
|
||||
|
||||
|
||||
@ -167,6 +167,7 @@ ReturnValue_t PdecHandler::irqOperation() {
|
||||
return result;
|
||||
}
|
||||
state = State::RUNNING;
|
||||
checkLocks();
|
||||
break;
|
||||
case State::RUNNING: {
|
||||
checkAndHandleIrqs(fd, info);
|
||||
|
||||
@ -25,6 +25,10 @@ static const Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
|
||||
static constexpr Event SAFE_RATE_RECOVERY = MAKE_EVENT(1, severity::MEDIUM);
|
||||
//!< Multiple RWs are invalid, not commandable and therefore higher ACS modes cannot be maintained.
|
||||
static constexpr Event MULTIPLE_RW_INVALID = MAKE_EVENT(2, severity::HIGH);
|
||||
//!< MEKF was not able to compute a solution.
|
||||
static constexpr Event MEKF_INVALID_INFO = MAKE_EVENT(3, severity::INFO);
|
||||
//!< MEKF was not able to compute a solution during any pointing ACS mode for a prolonged time.
|
||||
static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(4, severity::HIGH);
|
||||
|
||||
extern const char* getModeStr(AcsMode mode);
|
||||
|
||||
|
||||
@ -24,7 +24,7 @@ enum class CcsdsSubmode : uint8_t {
|
||||
DATARATE_HIGH = 2,
|
||||
DATARATE_DEFAULT = 3
|
||||
};
|
||||
enum class ParameterId : uint8_t { DATARATE = 0 };
|
||||
enum class ParameterId : uint8_t { DATARATE = 0, TRANSMITTER_TIMEOUT = 1 };
|
||||
|
||||
} // namespace com
|
||||
|
||||
|
||||
@ -14,8 +14,6 @@ AcsController::AcsController(object_id_t objectId)
|
||||
safeCtrl(&acsParameters),
|
||||
detumble(&acsParameters),
|
||||
ptgCtrl(&acsParameters),
|
||||
detumbleCounter{0},
|
||||
multipleRwUnavailableCounter{0},
|
||||
parameterHelper(this),
|
||||
mgmDataRaw(this),
|
||||
mgmDataProcessed(this),
|
||||
@ -28,6 +26,14 @@ AcsController::AcsController(object_id_t objectId)
|
||||
ctrlValData(this),
|
||||
actuatorCmdData(this) {}
|
||||
|
||||
ReturnValue_t AcsController::initialize() {
|
||||
ReturnValue_t result = parameterHelper.initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
return ExtendedControllerBase::initialize();
|
||||
}
|
||||
|
||||
ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
|
||||
ReturnValue_t result = actionHelper.handleActionMessage(message);
|
||||
if (result == returnvalue::OK) {
|
||||
@ -116,17 +122,24 @@ void AcsController::performSafe() {
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
// give desired satellite rate and sun direction to align
|
||||
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData);
|
||||
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
|
||||
if (not mekfInvalidFlag) {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
}
|
||||
} else {
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
// get desired satellite rate and sun direction to align
|
||||
double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0};
|
||||
guidance.getTargetParamsSafe(sunTargetDir, satRateSafe);
|
||||
// if MEKF is working
|
||||
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
||||
bool magMomMtqValid = false;
|
||||
if (validMekf == returnvalue::OK) {
|
||||
if (result == MultiplicativeKalmanFilter::KALMAN_RUNNING) {
|
||||
safeCtrl.safeMekf(now, mekfData.quatMekf.value, mekfData.quatMekf.isValid(),
|
||||
mgmDataProcessed.magIgrfModel.value, mgmDataProcessed.magIgrfModel.isValid(),
|
||||
susDataProcessed.sunIjkModel.value, susDataProcessed.isValid(),
|
||||
@ -141,24 +154,9 @@ void AcsController::performSafe() {
|
||||
sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid);
|
||||
}
|
||||
|
||||
int16_t cmdDipolMtqs[3] = {0, 0, 0};
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&ctrlValData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double unitQuat[4] = {0, 0, 0, 1};
|
||||
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
|
||||
ctrlValData.tgtQuat.setValid(false);
|
||||
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
|
||||
ctrlValData.errQuat.setValid(false);
|
||||
ctrlValData.errAng.value = errAng;
|
||||
ctrlValData.errAng.setValid(true);
|
||||
ctrlValData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
|
||||
// Detumble check and switch
|
||||
// detumble check and switch
|
||||
if (mekfData.satRotRateMekf.isValid() &&
|
||||
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
|
||||
acsParameters.detumbleParameter.omegaDetumbleStart) {
|
||||
@ -176,20 +174,8 @@ void AcsController::performSafe() {
|
||||
triggerEvent(acs::SAFE_RATE_VIOLATION);
|
||||
}
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&actuatorCmdData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
int32_t zeroVec[4] = {0, 0, 0, 0};
|
||||
std::memcpy(actuatorCmdData.rwTargetTorque.value, zeroVec, 4 * sizeof(int32_t));
|
||||
actuatorCmdData.rwTargetTorque.setValid(false);
|
||||
std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t));
|
||||
actuatorCmdData.rwTargetSpeed.setValid(false);
|
||||
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
|
||||
actuatorCmdData.mtqTargetDipole.setValid(true);
|
||||
actuatorCmdData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
|
||||
updateCtrlValData(errAng);
|
||||
updateActuatorCmdData(cmdDipolMtqs);
|
||||
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
|
||||
// acsParameters.rwHandlingParameters.rampTime);
|
||||
@ -201,15 +187,21 @@ void AcsController::performDetumble() {
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData);
|
||||
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
|
||||
if (not mekfInvalidFlag) {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
}
|
||||
} else {
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
double magMomMtq[3] = {0, 0, 0};
|
||||
detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value,
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), magMomMtq);
|
||||
int16_t cmdDipolMtqs[3] = {0, 0, 0};
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs);
|
||||
|
||||
if (mekfData.satRotRateMekf.isValid() &&
|
||||
@ -229,19 +221,8 @@ void AcsController::performDetumble() {
|
||||
triggerEvent(acs::SAFE_RATE_RECOVERY);
|
||||
}
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&actuatorCmdData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memset(actuatorCmdData.rwTargetTorque.value, 0, 4 * sizeof(double));
|
||||
actuatorCmdData.rwTargetTorque.setValid(false);
|
||||
std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t));
|
||||
actuatorCmdData.rwTargetSpeed.setValid(false);
|
||||
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
|
||||
actuatorCmdData.mtqTargetDipole.setValid(true);
|
||||
actuatorCmdData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
|
||||
disableCtrlValData();
|
||||
updateActuatorCmdData(cmdDipolMtqs);
|
||||
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
// acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0,
|
||||
// acsParameters.rwHandlingParameters.rampTime);
|
||||
@ -253,23 +234,34 @@ void AcsController::performPointingCtrl() {
|
||||
|
||||
sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
|
||||
&gyrDataProcessed, &gpsDataProcessed, &acsParameters);
|
||||
ReturnValue_t validMekf;
|
||||
navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed,
|
||||
&mekfData, &validMekf);
|
||||
|
||||
double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
|
||||
double quatRef[4] = {0, 0, 0, 0};
|
||||
ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
|
||||
&susDataProcessed, &mekfData);
|
||||
if (result != MultiplicativeKalmanFilter::KALMAN_RUNNING &&
|
||||
result != MultiplicativeKalmanFilter::KALMAN_INITIALIZED) {
|
||||
if (not mekfInvalidFlag) {
|
||||
triggerEvent(acs::MEKF_INVALID_INFO);
|
||||
mekfInvalidFlag = true;
|
||||
}
|
||||
if (mekfInvalidCounter > 4) {
|
||||
triggerEvent(acs::MEKF_INVALID_MODE_VIOLATION);
|
||||
}
|
||||
mekfInvalidCounter++;
|
||||
commandActuators(0, 0, 0, acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
|
||||
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
||||
acsParameters.rwHandlingParameters.rampTime);
|
||||
return;
|
||||
} else {
|
||||
mekfInvalidFlag = false;
|
||||
mekfInvalidCounter = 0;
|
||||
}
|
||||
uint8_t enableAntiStiction = true;
|
||||
|
||||
double quatErrorComplete[4] = {0, 0, 0, 0}, quatError[3] = {0, 0, 0},
|
||||
deltaRate[3] = {0, 0, 0}; // ToDo: check if pointer needed
|
||||
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
||||
result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
|
||||
if (result == returnvalue::FAILED) {
|
||||
multipleRwUnavailableCounter++;
|
||||
if (multipleRwUnavailableCounter > 4) {
|
||||
triggerEvent(acs::MULTIPLE_RW_INVALID);
|
||||
}
|
||||
multipleRwUnavailableCounter++;
|
||||
return;
|
||||
} else {
|
||||
multipleRwUnavailableCounter = 0;
|
||||
@ -278,40 +270,37 @@ void AcsController::performPointingCtrl() {
|
||||
double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0};
|
||||
double mgtDpDes[3] = {0, 0, 0};
|
||||
|
||||
// Variables required for guidance
|
||||
double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
|
||||
errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
|
||||
switch (submode) {
|
||||
case acs::PTG_IDLE:
|
||||
guidance.sunQuatPtg(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
|
||||
targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
|
||||
4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
|
||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
|
||||
actuatorCmd.scalingTorqueRws(torqueRws, torqueRwsScaled);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
&acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
|
||||
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
case acs::PTG_TARGET:
|
||||
guidance.targetQuatPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
|
||||
refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
|
||||
4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
|
||||
guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
||||
targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
|
||||
acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
@ -324,17 +313,15 @@ void AcsController::performPointingCtrl() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
case acs::PTG_TARGET_GS:
|
||||
guidance.targetQuatPtgGs(&sensorValues, &mekfData, &susDataProcessed, &gpsDataProcessed, now,
|
||||
targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.targetModeControllerParameters.quatRef,
|
||||
4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, quatError, deltaRate,
|
||||
guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
|
||||
susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
|
||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
@ -347,16 +334,18 @@ void AcsController::performPointingCtrl() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
case acs::PTG_NADIR:
|
||||
guidance.quatNadirPtgThreeAxes(&sensorValues, &gpsDataProcessed, &mekfData, now, targetQuat,
|
||||
refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.nadirModeControllerParameters.quatRef, 4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, quatError, deltaRate,
|
||||
guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
|
||||
gpsDataProcessed.gpsVelocity.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
||||
targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
|
||||
acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
@ -369,16 +358,17 @@ void AcsController::performPointingCtrl() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
case acs::PTG_INERTIAL:
|
||||
guidance.inertialQuatPtg(targetQuat, refSatRate);
|
||||
std::memcpy(quatRef, acsParameters.inertialModeControllerParameters.quatRef,
|
||||
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
|
||||
4 * sizeof(double));
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
guidance.comparePtg(targetQuat, &mekfData, quatRef, refSatRate, quatErrorComplete, quatError,
|
||||
deltaRate);
|
||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, quatError, deltaRate,
|
||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
||||
targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
|
||||
acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
@ -391,6 +381,7 @@ void AcsController::performPointingCtrl() {
|
||||
mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
|
||||
&(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
|
||||
&(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -398,24 +389,14 @@ void AcsController::performPointingCtrl() {
|
||||
ptgCtrl.rwAntistiction(&sensorValues, torqueRwsScaled);
|
||||
}
|
||||
|
||||
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
|
||||
actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value,
|
||||
sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value,
|
||||
sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws);
|
||||
int16_t cmdDipolMtqs[3] = {0, 0, 0};
|
||||
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs);
|
||||
|
||||
{
|
||||
PoolReadGuard pg(&actuatorCmdData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double));
|
||||
std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t));
|
||||
std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t));
|
||||
actuatorCmdData.setValidity(true, true);
|
||||
}
|
||||
}
|
||||
|
||||
updateCtrlValData(targetQuat, errorQuat, errorAngle);
|
||||
updateActuatorCmdData(rwTrqNs, cmdSpeedRws, cmdDipolMtqs);
|
||||
// commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
// acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0],
|
||||
// cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
||||
@ -451,6 +432,67 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole,
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void AcsController::updateActuatorCmdData(int16_t mtqTargetDipole[3]) {
|
||||
double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0};
|
||||
int32_t rwTargetSpeed[4] = {0, 0, 0, 0};
|
||||
updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole);
|
||||
}
|
||||
|
||||
void AcsController::updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
|
||||
int16_t mtqTargetDipole[3]) {
|
||||
{
|
||||
PoolReadGuard pg(&actuatorCmdData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
|
||||
std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
|
||||
std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
|
||||
actuatorCmdData.setValidity(true, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::updateCtrlValData(double errAng) {
|
||||
double unitQuat[4] = {0, 0, 0, 1};
|
||||
{
|
||||
PoolReadGuard pg(&ctrlValData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
|
||||
ctrlValData.tgtQuat.setValid(false);
|
||||
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
|
||||
ctrlValData.errQuat.setValid(false);
|
||||
ctrlValData.errAng.value = errAng;
|
||||
ctrlValData.errAng.setValid(true);
|
||||
ctrlValData.setValidity(true, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng) {
|
||||
{
|
||||
PoolReadGuard pg(&ctrlValData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
|
||||
std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
|
||||
ctrlValData.errAng.value = errAng;
|
||||
ctrlValData.setValidity(true, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AcsController::disableCtrlValData() {
|
||||
double unitQuat[4] = {0, 0, 0, 1};
|
||||
double errAng = 0;
|
||||
{
|
||||
PoolReadGuard pg(&ctrlValData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double));
|
||||
std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double));
|
||||
ctrlValData.errAng.value = errAng;
|
||||
ctrlValData.setValidity(false, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
||||
LocalDataPoolManager &poolManager) {
|
||||
// MGM Raw
|
||||
@ -524,11 +566,13 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
|
||||
// MEKF
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
|
||||
poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), false, 5.0});
|
||||
// Ctrl Values
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
|
||||
poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), false, 5.0});
|
||||
// Actuator CMD
|
||||
localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
|
||||
@ -795,11 +839,3 @@ void AcsController::copyGyrData() {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t AcsController::initialize() {
|
||||
ReturnValue_t result = parameterHelper.initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
return ExtendedControllerBase::initialize();
|
||||
}
|
||||
|
||||
@ -10,6 +10,7 @@
|
||||
|
||||
#include "acs/ActuatorCmd.h"
|
||||
#include "acs/Guidance.h"
|
||||
#include "acs/MultiplicativeKalmanFilter.h"
|
||||
#include "acs/Navigation.h"
|
||||
#include "acs/SensorProcessing.h"
|
||||
#include "acs/control/Detumble.h"
|
||||
@ -49,11 +50,15 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
Detumble detumble;
|
||||
PtgCtrl ptgCtrl;
|
||||
|
||||
uint8_t detumbleCounter;
|
||||
uint8_t multipleRwUnavailableCounter;
|
||||
|
||||
ParameterHelper parameterHelper;
|
||||
|
||||
uint8_t detumbleCounter = 0;
|
||||
uint8_t multipleRwUnavailableCounter = 0;
|
||||
bool mekfInvalidFlag = true;
|
||||
uint8_t mekfInvalidCounter = 0;
|
||||
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
|
||||
int16_t cmdDipolMtqs[3] = {0, 0, 0};
|
||||
|
||||
#if OBSW_THREAD_TRACING == 1
|
||||
uint32_t opCounter = 0;
|
||||
#endif
|
||||
@ -79,6 +84,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
|
||||
uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed,
|
||||
int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime);
|
||||
void updateActuatorCmdData(int16_t mtqTargetDipole[3]);
|
||||
void updateActuatorCmdData(double rwTargetTorque[4], int32_t rwTargetSpeed[4],
|
||||
int16_t mtqTargetDipole[3]);
|
||||
void updateCtrlValData(double errAng);
|
||||
void updateCtrlValData(double tgtQuat[4], double errQuat[4], double errAng);
|
||||
void disableCtrlValData();
|
||||
|
||||
/* ACS Sensor Values */
|
||||
ACS::SensorValues sensorValues;
|
||||
@ -169,12 +180,14 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
acsctrl::MekfData mekfData;
|
||||
PoolEntry<double> quatMekf = PoolEntry<double>(4);
|
||||
PoolEntry<double> satRotRateMekf = PoolEntry<double>(3);
|
||||
PoolEntry<uint8_t> mekfStatus = PoolEntry<uint8_t>();
|
||||
|
||||
// Ctrl Values
|
||||
acsctrl::CtrlValData ctrlValData;
|
||||
PoolEntry<double> tgtQuat = PoolEntry<double>(4);
|
||||
PoolEntry<double> errQuat = PoolEntry<double>(4);
|
||||
PoolEntry<double> errAng = PoolEntry<double>();
|
||||
PoolEntry<double> tgtRotRate = PoolEntry<double>(4);
|
||||
|
||||
// Actuator CMD
|
||||
acsctrl::ActuatorCmdData actuatorCmdData;
|
||||
|
||||
@ -342,7 +342,41 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0x9): // TargetModeControllerParameters
|
||||
case (0x9): // IdleModeControllerParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(targetModeControllerParameters.zeta);
|
||||
break;
|
||||
case 0x1:
|
||||
parameterWrapper->set(targetModeControllerParameters.om);
|
||||
break;
|
||||
case 0x2:
|
||||
parameterWrapper->set(targetModeControllerParameters.omMax);
|
||||
break;
|
||||
case 0x3:
|
||||
parameterWrapper->set(targetModeControllerParameters.qiMin);
|
||||
break;
|
||||
case 0x4:
|
||||
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0xA): // TargetModeControllerParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(targetModeControllerParameters.zeta);
|
||||
@ -408,7 +442,61 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0xA): // NadirModeControllerParameters
|
||||
case (0xB): // GsTargetModeControllerParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(targetModeControllerParameters.zeta);
|
||||
break;
|
||||
case 0x1:
|
||||
parameterWrapper->set(targetModeControllerParameters.om);
|
||||
break;
|
||||
case 0x2:
|
||||
parameterWrapper->set(targetModeControllerParameters.omMax);
|
||||
break;
|
||||
case 0x3:
|
||||
parameterWrapper->set(targetModeControllerParameters.qiMin);
|
||||
break;
|
||||
case 0x4:
|
||||
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->set(targetModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->set(targetModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->set(targetModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->set(targetModeControllerParameters.quatRef);
|
||||
break;
|
||||
case 0xC:
|
||||
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
case 0xD:
|
||||
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
|
||||
break;
|
||||
case 0xE:
|
||||
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
|
||||
break;
|
||||
case 0xF:
|
||||
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
|
||||
break;
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0xC): // NadirModeControllerParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(nadirModeControllerParameters.zeta);
|
||||
@ -450,7 +538,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0xB): // InertialModeControllerParameters
|
||||
case (0xD): // InertialModeControllerParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(inertialModeControllerParameters.zeta);
|
||||
@ -492,7 +580,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0xC): // StrParameters
|
||||
case (0xE): // StrParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(strParameters.exclusionAngle);
|
||||
@ -504,7 +592,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0xD): // GpsParameters
|
||||
case (0xF): // GpsParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
|
||||
@ -513,7 +601,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0xE): // SunModelParameters
|
||||
case (0x10): // SunModelParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(sunModelParameters.domega);
|
||||
@ -543,7 +631,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0xF): // KalmanFilterParameters
|
||||
case (0x11): // KalmanFilterParameters
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(kalmanFilterParameters.sensorNoiseSTR);
|
||||
@ -567,7 +655,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0x10): // MagnetorquesParameter
|
||||
case (0x12): // MagnetorquesParameter
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(magnetorquesParameter.mtq0orientationMatrix);
|
||||
@ -594,7 +682,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
break;
|
||||
case (0x11): // DetumbleParameter
|
||||
case (0x13): // DetumbleParameter
|
||||
switch (parameterId) {
|
||||
case 0x0:
|
||||
parameterWrapper->set(detumbleParameter.detumblecounter);
|
||||
|
||||
@ -841,10 +841,12 @@ class AcsParameters : public HasParametersIF {
|
||||
uint8_t enableAntiStiction = true;
|
||||
} pointingLawParameters;
|
||||
|
||||
struct IdleModeControllerParameters : PointingLawParameters {
|
||||
} idleModeControllerParameters;
|
||||
|
||||
struct TargetModeControllerParameters : PointingLawParameters {
|
||||
double refDirection[3] = {-1, 0, 0}; // Antenna
|
||||
double refRotRate[3] = {0, 0, 0}; // Not used atm, do we want an option to
|
||||
// give this as an input- currently en calculation is done
|
||||
double refRotRate[3] = {0, 0, 0};
|
||||
double quatRef[4] = {0, 0, 0, 1};
|
||||
int8_t timeElapsedMax = 10; // rot rate calculations
|
||||
|
||||
@ -860,9 +862,20 @@ class AcsParameters : public HasParametersIF {
|
||||
double blindRotRate = 1 * M_PI / 180;
|
||||
} targetModeControllerParameters;
|
||||
|
||||
struct GsTargetModeControllerParameters : PointingLawParameters {
|
||||
double refDirection[3] = {-1, 0, 0}; // Antenna
|
||||
int8_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]
|
||||
} gsTargetModeControllerParameters;
|
||||
|
||||
struct NadirModeControllerParameters : PointingLawParameters {
|
||||
double refDirection[3] = {-1, 0, 0}; // Antenna
|
||||
double quatRef[4] = {0, 0, 0, 1};
|
||||
double refRotRate[3] = {0, 0, 0};
|
||||
int8_t timeElapsedMax = 10; // rot rate calculations
|
||||
} nadirModeControllerParameters;
|
||||
|
||||
|
||||
@ -1,10 +1,3 @@
|
||||
/*
|
||||
* Guidance.cpp
|
||||
*
|
||||
* Created on: 6 Jun 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
#include "Guidance.h"
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
@ -19,70 +12,46 @@
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
Guidance::Guidance(AcsParameters *acsParameters_) { acsParameters = *acsParameters_; }
|
||||
Guidance::Guidance(AcsParameters *acsParameters_) : acsParameters(*acsParameters_) {}
|
||||
|
||||
Guidance::~Guidance() {}
|
||||
|
||||
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
|
||||
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
|
||||
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
|
||||
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
|
||||
3 * sizeof(double));
|
||||
} else {
|
||||
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3],
|
||||
double sunDirI[3], double refDirB[3], double quatBI[4],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to groundstation or given latitude, longitude and altitude
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
|
||||
// fixed/centered frame)
|
||||
double targetCart[3] = {0, 0, 0};
|
||||
// transform longitude, latitude and altitude to ECEF
|
||||
double targetE[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
|
||||
|
||||
// Position of the satellite in the earth/fixed frame via GPS
|
||||
double posSatE[3] = {0, 0, 0};
|
||||
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
|
||||
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
|
||||
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
|
||||
sensorValues->gpsSet.altitude.value, posSatE);
|
||||
|
||||
// Target direction in the ECEF frame
|
||||
// target direction in the ECEF frame
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
|
||||
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// Transformation between ECEF and Body frame
|
||||
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
// transformation between ECEF and Body frame
|
||||
double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
||||
|
||||
QuaternionOperations::toDcm(quatBJ, dcmBJ);
|
||||
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
|
||||
QuaternionOperations::toDcm(quatBI, dcmBI);
|
||||
MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
|
||||
|
||||
// Target Direction in the body frame
|
||||
// target Direction in the body frame
|
||||
double targetDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
|
||||
|
||||
@ -106,15 +75,13 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
|
||||
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
// calculation of reference rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
double velSatE[3] = {0, 0, 0};
|
||||
std::memcpy(velSatE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
|
||||
double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0}, velSatBPart2[3] = {0, 0, 0};
|
||||
// Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
|
||||
// velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
|
||||
MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
|
||||
double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiply(*dcmBI, *dcmIEDot, *dcmBEDot, 3, 3, 3);
|
||||
MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);
|
||||
|
||||
@ -124,21 +91,14 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
|
||||
double satRateDir[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
|
||||
VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
|
||||
VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3);
|
||||
VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, targetSatRotRate, 3);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate in case of star tracker blinding
|
||||
//-------------------------------------------------------------------------------------
|
||||
if (acsParameters.targetModeControllerParameters.avoidBlindStr) {
|
||||
double sunDirB[3] = {0, 0, 0};
|
||||
|
||||
if (susDataProcessed->sunIjkModel.isValid()) {
|
||||
double sunDirJ[3] = {0, 0, 0};
|
||||
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
|
||||
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
|
||||
} else {
|
||||
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
|
||||
}
|
||||
MatrixOperations<double>::multiply(*dcmBI, sunDirI, sunDirB, 3, 3, 1);
|
||||
|
||||
double exclAngle = acsParameters.strParameters.exclusionAngle,
|
||||
blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
|
||||
@ -148,18 +108,13 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
|
||||
|
||||
if (!(strBlindAvoidFlag)) {
|
||||
double critSightAngle = blindStart * exclAngle;
|
||||
|
||||
if (sightAngleSun < critSightAngle) {
|
||||
strBlindAvoidFlag = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
else {
|
||||
} else {
|
||||
if (sightAngleSun < blindEnd * exclAngle) {
|
||||
double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
|
||||
double blindRefRate[3] = {0, 0, 0};
|
||||
|
||||
if (sunDirB[1] < 0) {
|
||||
blindRefRate[0] = normBlindRefRate;
|
||||
blindRefRate[1] = 0;
|
||||
@ -169,21 +124,353 @@ void Guidance::targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl:
|
||||
blindRefRate[1] = 0;
|
||||
blindRefRate[2] = 0;
|
||||
}
|
||||
|
||||
VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);
|
||||
|
||||
VectorOperations<double>::add(blindRefRate, targetSatRotRate, targetSatRotRate, 3);
|
||||
} else {
|
||||
strBlindAvoidFlag = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
// revert calculated quaternion from qBX to qIX
|
||||
double quatIB[4] = {0, 0, 0, 1};
|
||||
QuaternionOperations::inverse(quatBI, quatIB);
|
||||
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
|
||||
}
|
||||
|
||||
void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *refSatRate) {
|
||||
void Guidance::targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3],
|
||||
double targetQuat[4], double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for target pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transform longitude, latitude and altitude to cartesian coordiantes (ECEF)
|
||||
double targetE[3] = {0, 0, 0};
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, targetE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetE, posSatE, targetDirE, 3);
|
||||
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// target direction in the ECI frame
|
||||
double posSatI[3] = {0, 0, 0}, targetI[3] = {0, 0, 0}, targetDirI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIE, targetE, targetI, 3, 3, 1);
|
||||
VectorOperations<double>::subtract(targetI, posSatI, targetDirI, 3);
|
||||
|
||||
// x-axis aligned with target direction
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirI, xAxis, 3);
|
||||
|
||||
// transform velocity into inertial frame
|
||||
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
|
||||
|
||||
// orbital normal vector of target and velocity vector
|
||||
double orbitalNormalI[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(posSatI, velocityI, orbitalNormalI);
|
||||
VectorOperations<double>::normalize(orbitalNormalI, orbitalNormalI, 3);
|
||||
|
||||
// y-axis of satellite in orbit plane so that z-axis is parallel to long side of picture
|
||||
// resolution
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalI, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// z-axis completes RHS
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmIX[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
QuaternionOperations::fromDcm(dcmIX, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], 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 groundStationE[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.gsTargetModeControllerParameters.latitudeTgt,
|
||||
acsParameters.gsTargetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.gsTargetModeControllerParameters.altitudeTgt, groundStationE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationE, posSatE, targetDirE, 3);
|
||||
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// target direction in the ECI frame
|
||||
double posSatI[3] = {0, 0, 0}, groundStationI[3] = {0, 0, 0}, groundStationDirI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIE, groundStationE, groundStationI, 3, 3, 1);
|
||||
VectorOperations<double>::subtract(groundStationI, posSatI, groundStationDirI, 3);
|
||||
|
||||
// negative x-axis aligned with target direction
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(groundStationDirI, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
|
||||
// get sun vector model in ECI
|
||||
VectorOperations<double>::normalize(sunDirI, sunDirI, 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(xAxis, sunDirI);
|
||||
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
|
||||
double sunParallel[3], zAxis[3];
|
||||
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
|
||||
VectorOperations<double>::subtract(sunDirI, sunParallel, zAxis, 3);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
|
||||
// y-axis completes RHS
|
||||
double yAxis[3];
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters.gsTargetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to sun
|
||||
//-------------------------------------------------------------------------------------
|
||||
// positive z-Axis of EIVE in direction of sun
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirI, zAxis, 3);
|
||||
|
||||
// assign helper vector (north pole inertial)
|
||||
double helperVec[3] = {0, 0, 1};
|
||||
|
||||
// construct y-axis from helper vector and z-axis
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// x-axis completes RHS
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
|
||||
VectorOperations<double>::normalize(xAxis, xAxis, 3);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//----------------------------------------------------------------------------
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
|
||||
double targetQuat[4], double refDirB[3],
|
||||
double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
||||
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// transformation between ECEF and Body frame
|
||||
double dcmBI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
QuaternionOperations::toDcm(quatBI, dcmBI);
|
||||
MatrixOperations<double>::multiply(*dcmBI, *dcmIE, *dcmBE, 3, 3, 3);
|
||||
|
||||
// target Direction in the body frame
|
||||
double targetDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
|
||||
|
||||
// rotation quaternion from two vectors
|
||||
double refDir[3] = {0, 0, 0};
|
||||
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
|
||||
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
|
||||
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
|
||||
double noramlizedTargetDirB[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
|
||||
VectorOperations<double>::normalize(refDir, refDir, 3);
|
||||
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
||||
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
||||
double crossDir[3] = {0, 0, 0};
|
||||
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
||||
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
|
||||
targetQuat[0] = crossDir[0];
|
||||
targetQuat[1] = crossDir[1];
|
||||
targetQuat[2] = crossDir[2];
|
||||
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
|
||||
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
|
||||
// revert calculated quaternion from qBX to qIX
|
||||
double quatIB[4] = {0, 0, 0, 1};
|
||||
QuaternionOperations::inverse(quatBI, quatIB);
|
||||
QuaternionOperations::multiply(quatIB, targetQuat, targetQuat);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// transformation between ECEF and ECI frame
|
||||
double dcmEI[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmIE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEIDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEI, *dcmEIDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEI, *dcmIE);
|
||||
|
||||
double dcmIEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEIDot, *dcmIEDot);
|
||||
|
||||
// satellite position in inertial reference frame
|
||||
double posSatI[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, posSatE, posSatI, 3, 3, 1);
|
||||
|
||||
// negative x-axis aligned with position vector
|
||||
// this aligns with the camera, E- and S-band antennas
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(posSatI, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
|
||||
// make z-Axis parallel to major part of camera resolution
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
double velocityI[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmIE, velSatE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmIEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityI, 3);
|
||||
VectorOperations<double>::cross(xAxis, velocityI, zAxis);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
|
||||
// y-Axis completes RHS
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
|
||||
// join transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
QuaternionOperations::fromDcm(dcmTgt, targetQuat);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, now, targetQuat, refSatRate);
|
||||
}
|
||||
|
||||
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) {
|
||||
// First calculate error quaternion between current and target orientation
|
||||
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
|
||||
// Last calculate add rotation from reference quaternion
|
||||
QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
|
||||
// Keep scalar part of quaternion positive
|
||||
if (errorQuat[3] < 0) {
|
||||
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
|
||||
}
|
||||
// Calculate error angle
|
||||
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
|
||||
|
||||
// Only give back error satellite rotational rate if orientation has already been aquired
|
||||
if (errorAngle < 2. / 180. * M_PI) {
|
||||
// First combine the target and reference satellite rotational rates
|
||||
double combinedRefSatRotRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
|
||||
// Then substract the combined required satellite rotational rates from the actual rate
|
||||
VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate,
|
||||
3);
|
||||
} else {
|
||||
// If orientation has not been aquired yet set satellite rotational rate to zero
|
||||
errorSatRotRate = 0;
|
||||
}
|
||||
|
||||
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
|
||||
// under 150 arcsec ??
|
||||
}
|
||||
|
||||
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double errorQuat[4],
|
||||
double errorSatRotRate[3], double errorAngle) {
|
||||
// First calculate error quaternion between current and target orientation
|
||||
QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
|
||||
// Keep scalar part of quaternion positive
|
||||
if (errorQuat[3] < 0) {
|
||||
VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);
|
||||
}
|
||||
// Calculate error angle
|
||||
errorAngle = QuaternionOperations::getAngle(errorQuat, true);
|
||||
|
||||
// Only give back error satellite rotational rate if orientation has already been aquired
|
||||
if (errorAngle < 2. / 180. * M_PI) {
|
||||
// Then substract the combined required satellite rotational rates from the actual rate
|
||||
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
|
||||
} else {
|
||||
// If orientation has not been aquired yet set satellite rotational rate to zero
|
||||
errorSatRotRate = 0;
|
||||
}
|
||||
|
||||
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
|
||||
// under 150 arcsec ??
|
||||
}
|
||||
|
||||
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *refSatRate) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) -
|
||||
(timeSavedQuaternion.tv_sec +
|
||||
timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6));
|
||||
@ -221,395 +508,6 @@ void Guidance::refRotationRate(int8_t timeElapsedMax, timeval now, double quatIn
|
||||
savedQuaternion[3] = quatInertialTarget[3];
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
acsctrl::MekfData *mekfData, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for target pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
|
||||
// fixed/centered frame)
|
||||
double targetCart[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, targetCart);
|
||||
// Position of the satellite in the earth/fixed frame via GPS
|
||||
double posSatE[3] = {0, 0, 0};
|
||||
std::memcpy(posSatE, gpsDataProcessed->gpsPosition.value, 3 * sizeof(double));
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(targetCart, posSatE, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
||||
|
||||
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
|
||||
// Target Direction and position vector in the inertial frame
|
||||
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
|
||||
|
||||
// negative x-Axis aligned with target (Camera/E-band transmitter position)
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
|
||||
// Transform velocity into inertial frame
|
||||
double velocityE[3];
|
||||
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
|
||||
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
|
||||
|
||||
// orbital normal vector
|
||||
double orbitalNormalJ[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(posSatJ, velocityJ, orbitalNormalJ);
|
||||
VectorOperations<double>::normalize(orbitalNormalJ, orbitalNormalJ, 3);
|
||||
|
||||
// y-Axis of satellite in orbit plane so that z-axis parallel to long side of picture resolution
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(orbitalNormalJ, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// z-Axis completes RHS
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(xAxis, yAxis, zAxis);
|
||||
|
||||
// Complete transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
double quatInertialTarget[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
|
||||
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
|
||||
|
||||
// Transform in system relative to satellite frame
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
||||
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for ground station pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Transform longitude, latitude and altitude to cartesian coordiantes (earth
|
||||
// fixed/centered frame)
|
||||
double groundStationCart[3] = {0, 0, 0};
|
||||
|
||||
MathOperations<double>::cartesianFromLatLongAlt(
|
||||
acsParameters.targetModeControllerParameters.latitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.longitudeTgt,
|
||||
acsParameters.targetModeControllerParameters.altitudeTgt, groundStationCart);
|
||||
// Position of the satellite in the earth/fixed frame via GPS
|
||||
double posSatE[3] = {0, 0, 0};
|
||||
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
|
||||
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
|
||||
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
|
||||
sensorValues->gpsSet.altitude.value, posSatE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
||||
|
||||
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
|
||||
// Target Direction and position vector in the inertial frame
|
||||
double targetDirJ[3] = {0, 0, 0}, posSatJ[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmJE, posSatE, posSatJ, 3, 3, 1);
|
||||
|
||||
// negative x-Axis aligned with target (Camera/E-band transmitter position)
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
|
||||
// get Sun Vector Model in ECI
|
||||
double sunJ[3];
|
||||
std::memcpy(sunJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
|
||||
VectorOperations<double>::normalize(sunJ, sunJ, 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(xAxis, sunJ);
|
||||
xDotS /= pow(VectorOperations<double>::norm(xAxis, 3), 2);
|
||||
double sunParallel[3], zAxis[3];
|
||||
VectorOperations<double>::mulScalar(xAxis, xDotS, sunParallel, 3);
|
||||
VectorOperations<double>::subtract(sunJ, sunParallel, zAxis, 3);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
|
||||
// calculate y-axis
|
||||
double yAxis[3];
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
// Complete transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
double quatInertialTarget[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters.targetModeControllerParameters.timeElapsedMax;
|
||||
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
|
||||
|
||||
// Transform in system relative to satellite frame
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
||||
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
|
||||
}
|
||||
|
||||
void Guidance::sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to sun
|
||||
//-------------------------------------------------------------------------------------
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
||||
QuaternionOperations::toDcm(quatBJ, dcmBJ);
|
||||
|
||||
double sunDirJ[3] = {0, 0, 0}, sunDirB[3] = {0, 0, 0};
|
||||
if (susDataProcessed->sunIjkModel.isValid()) {
|
||||
std::memcpy(sunDirJ, susDataProcessed->sunIjkModel.value, 3 * sizeof(double));
|
||||
MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
|
||||
} else if (susDataProcessed->susVecTot.isValid()) {
|
||||
std::memcpy(sunDirB, susDataProcessed->susVecTot.value, 3 * sizeof(double));
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
||||
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
|
||||
// positive z-Axis of EIVE in direction of sun
|
||||
double zAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(sunDirB, zAxis, 3);
|
||||
|
||||
// Assign helper vector (north pole inertial)
|
||||
double helperVec[3] = {0, 0, 1};
|
||||
|
||||
//
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxis, helperVec, yAxis);
|
||||
VectorOperations<double>::normalize(yAxis, yAxis, 3);
|
||||
|
||||
//
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(yAxis, zAxis, xAxis);
|
||||
VectorOperations<double>::normalize(xAxis, xAxis, 3);
|
||||
|
||||
// Transformation matrix to Sun, no further transforamtions, reference is already
|
||||
// the EIVE body frame
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
double quatSun[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::fromDcm(dcmTgt, quatSun);
|
||||
|
||||
targetQuat[0] = quatSun[0];
|
||||
targetQuat[1] = quatSun[1];
|
||||
targetQuat[2] = quatSun[2];
|
||||
targetQuat[3] = quatSun[3];
|
||||
|
||||
//----------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//----------------------------------------------------------------------------
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
}
|
||||
|
||||
void Guidance::quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
timeval now, double targetQuat[4],
|
||||
double refSatRate[3]) { // old version of Nadir Pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Position of the satellite in the earth/fixed frame via GPS
|
||||
double posSatE[3] = {0, 0, 0};
|
||||
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
|
||||
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
|
||||
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
|
||||
sensorValues->gpsSet.altitude.value, posSatE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
||||
|
||||
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
|
||||
// Transformation between ECEF and Body frame
|
||||
double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
||||
QuaternionOperations::toDcm(quatBJ, dcmBJ);
|
||||
MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);
|
||||
|
||||
// Target Direction in the body frame
|
||||
double targetDirB[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);
|
||||
|
||||
// rotation quaternion from two vectors
|
||||
double refDir[3] = {0, 0, 0};
|
||||
refDir[0] = acsParameters.nadirModeControllerParameters.refDirection[0];
|
||||
refDir[1] = acsParameters.nadirModeControllerParameters.refDirection[1];
|
||||
refDir[2] = acsParameters.nadirModeControllerParameters.refDirection[2];
|
||||
double noramlizedTargetDirB[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
|
||||
VectorOperations<double>::normalize(refDir, refDir, 3);
|
||||
double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
|
||||
double normRefDir = VectorOperations<double>::norm(refDir, 3);
|
||||
double crossDir[3] = {0, 0, 0};
|
||||
double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
|
||||
VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
|
||||
targetQuat[0] = crossDir[0];
|
||||
targetQuat[1] = crossDir[1];
|
||||
targetQuat[2] = crossDir[2];
|
||||
targetQuat[3] = sqrt(pow(normTargetDirB, 2) * pow(normRefDir, 2) + dotDirections);
|
||||
VectorOperations<double>::normalize(targetQuat, targetQuat, 4);
|
||||
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//-------------------------------------------------------------------------------------
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
}
|
||||
|
||||
void Guidance::quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
|
||||
double refSatRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion for Nadir pointing
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Position of the satellite in the earth/fixed frame via GPS
|
||||
double posSatE[3] = {0, 0, 0};
|
||||
double geodeticLatRad = (sensorValues->gpsSet.latitude.value) * PI / 180;
|
||||
double longitudeRad = (sensorValues->gpsSet.longitude.value) * PI / 180;
|
||||
MathOperations<double>::cartesianFromLatLongAlt(geodeticLatRad, longitudeRad,
|
||||
sensorValues->gpsSet.altitude.value, posSatE);
|
||||
double targetDirE[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(posSatE, -1, targetDirE, 3);
|
||||
|
||||
// Transformation between ECEF and IJK frame
|
||||
double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::ecfToEciWithNutPre(now, *dcmEJ, *dcmEJDot);
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
|
||||
|
||||
double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);
|
||||
|
||||
// Target Direction in the body frame
|
||||
double targetDirJ[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmJE, targetDirE, targetDirJ, 3, 3, 1);
|
||||
|
||||
// negative x-Axis aligned with target (Camera position)
|
||||
double xAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::normalize(targetDirJ, xAxis, 3);
|
||||
VectorOperations<double>::mulScalar(xAxis, -1, xAxis, 3);
|
||||
|
||||
// z-Axis parallel to long side of picture resolution
|
||||
double zAxis[3] = {0, 0, 0}, velocityE[3];
|
||||
std::memcpy(velocityE, gpsDataProcessed->gpsVelocity.value, 3 * sizeof(double));
|
||||
double velocityJ[3] = {0, 0, 0}, velPart1[3] = {0, 0, 0}, velPart2[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*dcmJE, velocityE, velPart1, 3, 3, 1);
|
||||
MatrixOperations<double>::multiply(*dcmJEDot, posSatE, velPart2, 3, 3, 1);
|
||||
VectorOperations<double>::add(velPart1, velPart2, velocityJ, 3);
|
||||
VectorOperations<double>::cross(xAxis, velocityJ, zAxis);
|
||||
VectorOperations<double>::normalize(zAxis, zAxis, 3);
|
||||
|
||||
// y-Axis completes RHS
|
||||
double yAxis[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(zAxis, xAxis, yAxis);
|
||||
|
||||
// Complete transformation matrix
|
||||
double dcmTgt[3][3] = {{xAxis[0], yAxis[0], zAxis[0]},
|
||||
{xAxis[1], yAxis[1], zAxis[1]},
|
||||
{xAxis[2], yAxis[2], zAxis[2]}};
|
||||
double quatInertialTarget[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::fromDcm(dcmTgt, quatInertialTarget);
|
||||
|
||||
int8_t timeElapsedMax = acsParameters.nadirModeControllerParameters.timeElapsedMax;
|
||||
refRotationRate(timeElapsedMax, now, quatInertialTarget, refSatRate);
|
||||
|
||||
// Transform in system relative to satellite frame
|
||||
double quatBJ[4] = {0, 0, 0, 0};
|
||||
std::memcpy(quatBJ, mekfData->quatMekf.value, 4 * sizeof(double));
|
||||
QuaternionOperations::multiply(quatBJ, quatInertialTarget, targetQuat);
|
||||
}
|
||||
|
||||
void Guidance::inertialQuatPtg(double targetQuat[4], double refSatRate[3]) {
|
||||
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
|
||||
4 * sizeof(double));
|
||||
std::memcpy(refSatRate, acsParameters.inertialModeControllerParameters.refRotRate,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
|
||||
void Guidance::comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
|
||||
double refSatRate[3], double quatErrorComplete[4], double quatError[3],
|
||||
double deltaRate[3]) {
|
||||
double satRate[3] = {0, 0, 0};
|
||||
std::memcpy(satRate, mekfData->satRotRateMekf.value, 3 * sizeof(double));
|
||||
VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
|
||||
// valid checks ?
|
||||
double quatErrorMtx[4][4] = {{quatRef[3], quatRef[2], -quatRef[1], -quatRef[0]},
|
||||
{-quatRef[2], quatRef[3], quatRef[0], -quatRef[1]},
|
||||
{quatRef[1], -quatRef[0], quatRef[3], -quatRef[2]},
|
||||
{quatRef[0], -quatRef[1], quatRef[2], quatRef[3]}};
|
||||
|
||||
MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);
|
||||
|
||||
if (quatErrorComplete[3] < 0) {
|
||||
quatErrorComplete[3] *= -1;
|
||||
}
|
||||
|
||||
quatError[0] = quatErrorComplete[0];
|
||||
quatError[1] = quatErrorComplete[1];
|
||||
quatError[2] = quatErrorComplete[2];
|
||||
|
||||
// target flag in matlab, importance, does look like it only gives feedback if pointing control is
|
||||
// under 150 arcsec ??
|
||||
}
|
||||
|
||||
ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||
double *rwPseudoInv) {
|
||||
bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid());
|
||||
@ -640,3 +538,16 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {
|
||||
if (not std::filesystem::exists(SD_0_SKEWED_PTG_FILE) or
|
||||
not std::filesystem::exists(SD_1_SKEWED_PTG_FILE)) { // ToDo: if file does not exist anymore
|
||||
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir,
|
||||
3 * sizeof(double));
|
||||
} else {
|
||||
std::memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDirLeop,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
std::memcpy(satRateSafe, acsParameters.safeModeControllerParameters.satRateRef,
|
||||
3 * sizeof(double));
|
||||
}
|
||||
|
||||
@ -1,10 +1,3 @@
|
||||
/*
|
||||
* Guidance.h
|
||||
*
|
||||
* Created on: 6 Jun 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
#ifndef GUIDANCE_H_
|
||||
#define GUIDANCE_H_
|
||||
|
||||
@ -23,47 +16,38 @@ class Guidance {
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position and
|
||||
// position of the ground station
|
||||
void targetQuatPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
|
||||
double refSatRate[3]);
|
||||
void targetQuatPtgGs(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
void targetQuatPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now,
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
|
||||
double refDirB[3], double quatBI[4], double targetQuat[4],
|
||||
double targetSatRotRate[3]);
|
||||
void targetQuatPtgThreeAxes(timeval now, double posSatE[3], double velSatE[3], double quatIX[4],
|
||||
double targetSatRotRate[3]);
|
||||
void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4],
|
||||
double targetSatRotRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate for sun pointing after ground
|
||||
// station
|
||||
void sunQuatPtg(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed, timeval now, double targetQuat[4],
|
||||
double refSatRate[3]);
|
||||
void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
|
||||
// pointing
|
||||
void quatNadirPtgThreeAxes(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
acsctrl::MekfData *mekfData, timeval now, double targetQuat[4],
|
||||
double refSatRate[3]);
|
||||
void quatNadirPtgSingleAxis(ACS::SensorValues *sensorValues, acsctrl::MekfData *mekfData,
|
||||
timeval now, double targetQuat[4], double refSatRate[3]);
|
||||
void targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
|
||||
double targetQuat[4], double refDirB[3], double refSatRate[3]);
|
||||
void targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], double velSatE[3],
|
||||
double targetQuat[4], double refSatRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from parameters for inertial
|
||||
// pointing
|
||||
void inertialQuatPtg(double targetQuat[4], double refSatRate[3]);
|
||||
// @note: Calculates the error quaternion between the current orientation and the target
|
||||
// quaternion, considering a reference quaternion. Additionally the difference between the actual
|
||||
// and a desired satellite rotational rate is calculated, again considering a reference rotational
|
||||
// rate. Lastly gives back the error angle of the error quaternion.
|
||||
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);
|
||||
void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3],
|
||||
double errorAngle);
|
||||
|
||||
// @note: compares target Quaternion and reference quaternion, also actual satellite rate and
|
||||
// desired
|
||||
void comparePtg(double targetQuat[4], acsctrl::MekfData *mekfData, double quatRef[4],
|
||||
double refSatRate[3], double quatErrorComplete[4], double quatError[3],
|
||||
double deltaRate[3]);
|
||||
|
||||
void refRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *refSatRate);
|
||||
void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *targetSatRotRate);
|
||||
|
||||
// @note: will give back the pseudoinverse matrix for the reaction wheel depending on the valid
|
||||
// reation wheel maybe can be done in "commanding.h"
|
||||
|
||||
@ -14,7 +14,7 @@
|
||||
|
||||
/*Initialisation of values for parameters in constructor*/
|
||||
MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters_)
|
||||
: initialQuaternion{0.5, 0.5, 0.5, 0.5},
|
||||
: initialQuaternion{0, 0, 0, 1},
|
||||
initialCovarianceMatrix{{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}} {
|
||||
loadAcsParameters(acsParameters_);
|
||||
@ -27,12 +27,10 @@ void MultiplicativeKalmanFilter::loadAcsParameters(AcsParameters *acsParameters_
|
||||
kalmanFilterParameters = &(acsParameters_->kalmanFilterParameters);
|
||||
}
|
||||
|
||||
void MultiplicativeKalmanFilter::reset() {}
|
||||
|
||||
void MultiplicativeKalmanFilter::init(
|
||||
ReturnValue_t MultiplicativeKalmanFilter::init(
|
||||
const double *magneticField_, const bool validMagField_, const double *sunDir_,
|
||||
const bool validSS, const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
|
||||
const bool validMagModel) { // valids for "model measurements"?
|
||||
const bool validMagModel, acsctrl::MekfData *mekfData) { // valids for "model measurements"?
|
||||
// check for valid mag/sun
|
||||
if (validMagField_ && validSS && validSSModel && validMagModel) {
|
||||
validInit = true;
|
||||
@ -190,9 +188,13 @@ void MultiplicativeKalmanFilter::init(
|
||||
initialCovarianceMatrix[5][3] = initGyroCov[2][0];
|
||||
initialCovarianceMatrix[5][4] = initGyroCov[2][1];
|
||||
initialCovarianceMatrix[5][5] = initGyroCov[2][2];
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::INITIALIZED);
|
||||
return KALMAN_INITIALIZED;
|
||||
} else {
|
||||
// no initialisation possible, no valid measurements
|
||||
validInit = false;
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
|
||||
return KALMAN_UNINITIALIZED;
|
||||
}
|
||||
}
|
||||
|
||||
@ -208,33 +210,13 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
|
||||
// Check for GYR Measurements
|
||||
int MDF = 0; // Matrix Dimension Factor
|
||||
if (!validGYRs_) {
|
||||
{
|
||||
PoolReadGuard pg(mekfData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
|
||||
double zeroVec[3] = {0.0, 0.0, 0.0};
|
||||
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
|
||||
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
|
||||
mekfData->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
validMekf = false;
|
||||
return KALMAN_NO_GYR_MEAS;
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::NO_GYR_DATA);
|
||||
return KALMAN_NO_GYR_DATA;
|
||||
}
|
||||
// Check for Model Calculations
|
||||
else if (!validSSModel || !validMagModel) {
|
||||
{
|
||||
PoolReadGuard pg(mekfData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
|
||||
double zeroVec[3] = {0.0, 0.0, 0.0};
|
||||
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
|
||||
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
|
||||
mekfData->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
validMekf = false;
|
||||
return KALMAN_NO_MODEL;
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::NO_MODEL_VECTORS);
|
||||
return KALMAN_NO_MODEL_VECTORS;
|
||||
}
|
||||
// Check Measurements available from SS, MAG, STR
|
||||
if (validSS && validMagField_ && validSTR_) {
|
||||
@ -260,17 +242,7 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
|
||||
MDF = 3;
|
||||
} else {
|
||||
sensorsAvail = 8; // no measurements
|
||||
validMekf = false;
|
||||
{
|
||||
PoolReadGuard pg(mekfData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
|
||||
double zeroVec[3] = {0.0, 0.0, 0.0};
|
||||
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
|
||||
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
|
||||
mekfData->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::NO_SUS_MGM_STR_DATA);
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
|
||||
@ -881,18 +853,8 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
|
||||
double invResidualCov[MDF][MDF] = {{0}};
|
||||
int inversionFailed = MathOperations<double>::inverseMatrix(*residualCov, *invResidualCov, MDF);
|
||||
if (inversionFailed) {
|
||||
{
|
||||
PoolReadGuard pg(mekfData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
|
||||
double zeroVec[3] = {0.0, 0.0, 0.0};
|
||||
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
|
||||
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
|
||||
mekfData->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
validMekf = false;
|
||||
return KALMAN_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::COVARIANCE_INVERSION_FAILED);
|
||||
return KALMAN_COVARIANCE_INVERSION_FAILED; // RETURN VALUE ? -- Like: Kalman Inversion Failed
|
||||
}
|
||||
|
||||
// [K = P * H' / (H * P * H' + R)]
|
||||
@ -1121,20 +1083,46 @@ ReturnValue_t MultiplicativeKalmanFilter::mekfEst(const double *quaternionSTR, c
|
||||
MatrixOperations<double>::multiply(*discTimeMatrix, *cov1, *cov1, 6, 6, 6);
|
||||
|
||||
MatrixOperations<double>::add(*cov0, *cov1, *initialCovarianceMatrix, 6, 6);
|
||||
validMekf = true;
|
||||
|
||||
// Discrete Time Step
|
||||
updateDataSet(mekfData, MekfStatus::RUNNING, quatBJ, rotRateEst);
|
||||
return KALMAN_RUNNING;
|
||||
}
|
||||
|
||||
// Check for new data in measurement -> SensorProcessing ?
|
||||
void MultiplicativeKalmanFilter::reset(acsctrl::MekfData *mekfData) {
|
||||
double resetQuaternion[4] = {0, 0, 0, 1};
|
||||
double resetCovarianceMatrix[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
|
||||
{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
|
||||
std::memcpy(initialQuaternion, resetQuaternion, 4 * sizeof(double));
|
||||
std::memcpy(initialCovarianceMatrix, resetCovarianceMatrix, 6 * 6 * sizeof(double));
|
||||
updateDataSetWithoutData(mekfData, MekfStatus::UNINITIALIZED);
|
||||
}
|
||||
|
||||
void MultiplicativeKalmanFilter::updateDataSetWithoutData(acsctrl::MekfData *mekfData,
|
||||
MekfStatus mekfStatus) {
|
||||
{
|
||||
PoolReadGuard pg(mekfData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mekfData->quatMekf.value, quatBJ, 4 * sizeof(double));
|
||||
std::memcpy(mekfData->satRotRateMekf.value, rotRateEst, 3 * sizeof(double));
|
||||
double unitQuat[4] = {0.0, 0.0, 0.0, 1.0};
|
||||
double zeroVec[3] = {0.0, 0.0, 0.0};
|
||||
std::memcpy(mekfData->quatMekf.value, unitQuat, 4 * sizeof(double));
|
||||
mekfData->quatMekf.setValid(false);
|
||||
std::memcpy(mekfData->satRotRateMekf.value, zeroVec, 3 * sizeof(double));
|
||||
mekfData->satRotRateMekf.setValid(false);
|
||||
mekfData->mekfStatus = mekfStatus;
|
||||
mekfData->setValidity(true, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void MultiplicativeKalmanFilter::updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus,
|
||||
double quat[4], double satRotRate[3]) {
|
||||
{
|
||||
PoolReadGuard pg(mekfData);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(mekfData->quatMekf.value, quat, 4 * sizeof(double));
|
||||
std::memcpy(mekfData->satRotRateMekf.value, satRotRate, 3 * sizeof(double));
|
||||
mekfData->mekfStatus = mekfStatus;
|
||||
mekfData->setValidity(true, true);
|
||||
}
|
||||
}
|
||||
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
@ -15,8 +15,7 @@
|
||||
#ifndef MULTIPLICATIVEKALMANFILTER_H_
|
||||
#define MULTIPLICATIVEKALMANFILTER_H_
|
||||
|
||||
#include <stdint.h> //uint8_t
|
||||
#include <time.h> /*purpose, timeval ?*/
|
||||
#include <stdint.h>
|
||||
|
||||
#include "../controllerdefinitions/AcsCtrlDefinitions.h"
|
||||
#include "AcsParameters.h"
|
||||
@ -30,7 +29,7 @@ class MultiplicativeKalmanFilter {
|
||||
MultiplicativeKalmanFilter(AcsParameters *acsParameters_);
|
||||
virtual ~MultiplicativeKalmanFilter();
|
||||
|
||||
void reset(); // NOT YET DEFINED - should only reset all mekf variables
|
||||
void reset(acsctrl::MekfData *mekfData);
|
||||
|
||||
/* @brief: init() - This function initializes the Kalman Filter and will provide the first
|
||||
* quaternion through the QUEST algorithm
|
||||
@ -39,9 +38,10 @@ class MultiplicativeKalmanFilter {
|
||||
* sunDirJ sun direction vector in the ECI frame
|
||||
* magFieldJ magnetic field vector in the ECI frame
|
||||
*/
|
||||
void init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
|
||||
ReturnValue_t init(const double *magneticField_, const bool validMagField_, const double *sunDir_,
|
||||
const bool validSS, const double *sunDirJ, const bool validSSModel,
|
||||
const double *magFieldJ, const bool validMagModel);
|
||||
const double *magFieldJ, const bool validMagModel,
|
||||
acsctrl::MekfData *mekfData);
|
||||
|
||||
/* @brief: mekfEst() - This function calculates the quaternion and gyro bias of the Kalman Filter
|
||||
* for the current step after the initalization
|
||||
@ -63,11 +63,26 @@ class MultiplicativeKalmanFilter {
|
||||
const double *sunDirJ, const bool validSSModel, const double *magFieldJ,
|
||||
const bool validMagModel, double sampleTime, acsctrl::MekfData *mekfData);
|
||||
|
||||
enum MekfStatus : uint8_t {
|
||||
UNINITIALIZED = 0,
|
||||
NO_GYR_DATA = 1,
|
||||
NO_MODEL_VECTORS = 2,
|
||||
NO_SUS_MGM_STR_DATA = 3,
|
||||
COVARIANCE_INVERSION_FAILED = 4,
|
||||
INITIALIZED = 10,
|
||||
RUNNING = 11,
|
||||
};
|
||||
|
||||
// resetting Mekf
|
||||
static constexpr uint8_t IF_KAL_ID = CLASS_ID::ACS_KALMAN;
|
||||
static constexpr ReturnValue_t KALMAN_NO_GYR_MEAS = returnvalue::makeCode(IF_KAL_ID, 1);
|
||||
static constexpr ReturnValue_t KALMAN_NO_MODEL = returnvalue::makeCode(IF_KAL_ID, 2);
|
||||
static constexpr ReturnValue_t KALMAN_INVERSION_FAILED = returnvalue::makeCode(IF_KAL_ID, 3);
|
||||
static constexpr ReturnValue_t KALMAN_UNINITIALIZED = returnvalue::makeCode(IF_KAL_ID, 2);
|
||||
static constexpr ReturnValue_t KALMAN_NO_GYR_DATA = returnvalue::makeCode(IF_KAL_ID, 3);
|
||||
static constexpr ReturnValue_t KALMAN_NO_MODEL_VECTORS = returnvalue::makeCode(IF_KAL_ID, 4);
|
||||
static constexpr ReturnValue_t KALMAN_NO_SUS_MGM_STR_DATA = returnvalue::makeCode(IF_KAL_ID, 5);
|
||||
static constexpr ReturnValue_t KALMAN_COVARIANCE_INVERSION_FAILED =
|
||||
returnvalue::makeCode(IF_KAL_ID, 6);
|
||||
static constexpr ReturnValue_t KALMAN_INITIALIZED = returnvalue::makeCode(IF_KAL_ID, 7);
|
||||
static constexpr ReturnValue_t KALMAN_RUNNING = returnvalue::makeCode(IF_KAL_ID, 8);
|
||||
|
||||
private:
|
||||
/*Parameters*/
|
||||
@ -80,16 +95,17 @@ class MultiplicativeKalmanFilter {
|
||||
double initialQuaternion[4]; /*after reset?QUEST*/
|
||||
double initialCovarianceMatrix[6][6]; /*after reset?QUEST*/
|
||||
double propagatedQuaternion[4]; /*Filter Quaternion for next step*/
|
||||
bool validMekf;
|
||||
uint8_t sensorsAvail;
|
||||
|
||||
/*Outputs*/
|
||||
double quatBJ[4]; /* Output Quaternion */
|
||||
double biasGYR[3]; /*Between measured and estimated sat Rate*/
|
||||
/*Parameter INIT*/
|
||||
// double alpha, gamma, beta;
|
||||
/*Functions*/
|
||||
void loadAcsParameters(AcsParameters *acsParameters_);
|
||||
void updateDataSetWithoutData(acsctrl::MekfData *mekfData, MekfStatus mekfStatus);
|
||||
void updateDataSet(acsctrl::MekfData *mekfData, MekfStatus mekfStatus, double quat[4],
|
||||
double satRotRate[3]);
|
||||
};
|
||||
|
||||
#endif /* ACS_MULTIPLICATIVEKALMANFILTER_H_ */
|
||||
|
||||
@ -1,10 +1,3 @@
|
||||
/*
|
||||
* Navigation.cpp
|
||||
*
|
||||
* Created on: 23 May 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
#include "Navigation.h"
|
||||
|
||||
#include <fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
@ -21,37 +14,37 @@ Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilt
|
||||
|
||||
Navigation::~Navigation() {}
|
||||
|
||||
void Navigation::useMekf(ACS::SensorValues *sensorValues,
|
||||
ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
|
||||
ReturnValue_t *mekfValid) {
|
||||
double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
||||
acsctrl::SusDataProcessed *susDataProcessed,
|
||||
acsctrl::MekfData *mekfData) {
|
||||
double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
|
||||
sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
|
||||
bool quatJBValid = sensorValues->strSet.caliQx.isValid() &&
|
||||
bool quatIBValid = sensorValues->strSet.caliQx.isValid() &&
|
||||
sensorValues->strSet.caliQy.isValid() &&
|
||||
sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid();
|
||||
|
||||
if (kalmanInit) {
|
||||
*mekfValid = multiplicativeKalmanFilter.mekfEst(
|
||||
quatJB, quatJBValid, gyrDataProcessed->gyrVecTot.value,
|
||||
return multiplicativeKalmanFilter.mekfEst(
|
||||
quatIB, quatIBValid, gyrDataProcessed->gyrVecTot.value,
|
||||
gyrDataProcessed->gyrVecTot.isValid(), mgmDataProcessed->mgmVecTot.value,
|
||||
mgmDataProcessed->mgmVecTot.isValid(), susDataProcessed->susVecTot.value,
|
||||
susDataProcessed->susVecTot.isValid(), susDataProcessed->sunIjkModel.value,
|
||||
susDataProcessed->sunIjkModel.isValid(), mgmDataProcessed->magIgrfModel.value,
|
||||
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime,
|
||||
mekfData); // VALIDS FOR QUAT AND RATE ??
|
||||
mgmDataProcessed->magIgrfModel.isValid(), acsParameters.onBoardParams.sampleTime, mekfData);
|
||||
} else {
|
||||
multiplicativeKalmanFilter.init(
|
||||
ReturnValue_t result;
|
||||
result = multiplicativeKalmanFilter.init(
|
||||
mgmDataProcessed->mgmVecTot.value, mgmDataProcessed->mgmVecTot.isValid(),
|
||||
susDataProcessed->susVecTot.value, susDataProcessed->susVecTot.isValid(),
|
||||
susDataProcessed->sunIjkModel.value, susDataProcessed->sunIjkModel.isValid(),
|
||||
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid());
|
||||
mgmDataProcessed->magIgrfModel.value, mgmDataProcessed->magIgrfModel.isValid(), mekfData);
|
||||
kalmanInit = true;
|
||||
*mekfValid = returnvalue::OK;
|
||||
|
||||
// Maybe we need feedback from kalmanfilter to identify if there was a problem with the
|
||||
// init of kalman filter where does this class know from that kalman filter was not
|
||||
// initialized ?
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
|
||||
multiplicativeKalmanFilter.reset(mekfData);
|
||||
}
|
||||
|
||||
@ -1,10 +1,3 @@
|
||||
/*
|
||||
* Navigation.h
|
||||
*
|
||||
* Created on: 19 Apr 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATION_H_
|
||||
#define NAVIGATION_H_
|
||||
|
||||
@ -16,14 +9,14 @@
|
||||
|
||||
class Navigation {
|
||||
public:
|
||||
Navigation(AcsParameters *acsParameters_); // Input mode ?
|
||||
Navigation(AcsParameters *acsParameters_);
|
||||
virtual ~Navigation();
|
||||
|
||||
void useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
ReturnValue_t useMekf(ACS::SensorValues *sensorValues,
|
||||
acsctrl::GyrDataProcessed *gyrDataProcessed,
|
||||
acsctrl::MgmDataProcessed *mgmDataProcessed,
|
||||
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData,
|
||||
ReturnValue_t *mekfValid);
|
||||
void processSensorData();
|
||||
acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MekfData *mekfData);
|
||||
void resetMekf(acsctrl::MekfData *mekfData);
|
||||
|
||||
protected:
|
||||
private:
|
||||
|
||||
@ -1,10 +1,3 @@
|
||||
/*
|
||||
* SensorProcessing.cpp
|
||||
*
|
||||
* Created on: 7 Mar 2022
|
||||
* Author: Robin Marquardt
|
||||
*/
|
||||
|
||||
#include "SensorProcessing.h"
|
||||
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
|
||||
@ -63,8 +63,7 @@ void SusConverter::calcAngle(const uint16_t susChannel[6]) {
|
||||
}
|
||||
|
||||
void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
|
||||
uint8_t index;
|
||||
float k, l;
|
||||
uint8_t index, k, l;
|
||||
|
||||
// while loop iterates above all calibration cells to use the different calibration functions in
|
||||
// each cell
|
||||
@ -75,10 +74,10 @@ void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffB
|
||||
while (l < 3) {
|
||||
l++;
|
||||
// if-condition to check in which cell the data point has to be
|
||||
if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3)) - halfCellWidth) &&
|
||||
alphaBetaRaw[0] < ((completeCellWidth * (k / 3)) - halfCellWidth)) &&
|
||||
(alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3)) - halfCellWidth) &&
|
||||
alphaBetaRaw[1] < ((completeCellWidth * (l / 3)) - halfCellWidth))) {
|
||||
if ((alphaBetaRaw[0] > ((completeCellWidth * ((k - 1) / 3.)) - halfCellWidth) &&
|
||||
alphaBetaRaw[0] < ((completeCellWidth * (k / 3.)) - halfCellWidth)) &&
|
||||
(alphaBetaRaw[1] > ((completeCellWidth * ((l - 1) / 3.)) - halfCellWidth) &&
|
||||
alphaBetaRaw[1] < ((completeCellWidth * (l / 3.)) - halfCellWidth))) {
|
||||
index = (3 * (k - 1) + l) - 1; // calculate the index of the datapoint for the right cell
|
||||
alphaBetaCalibrated[0] =
|
||||
coeffAlpha[index][0] + coeffAlpha[index][1] * alphaBetaRaw[0] +
|
||||
|
||||
@ -27,7 +27,7 @@ void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_) {
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const double *qError, const double *deltaRate, const double *rwPseudoInv,
|
||||
const double *errorQuat, const double *deltaRate, const double *rwPseudoInv,
|
||||
double *torqueRws) {
|
||||
//------------------------------------------------------------------------------------------------
|
||||
// Compute gain matrix K and P matrix
|
||||
@ -37,6 +37,8 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
||||
double qErrorMin = pointingLawParameters->qiMin;
|
||||
double omMax = pointingLawParameters->omMax;
|
||||
|
||||
double qError[3] = {errorQuat[0], errorQuat[1], errorQuat[2]};
|
||||
|
||||
double cInt = 2 * om * zeta;
|
||||
double kInt = 2 * pow(om, 2);
|
||||
|
||||
|
||||
@ -91,10 +91,12 @@ enum PoolIds : lp_id_t {
|
||||
// MEKF
|
||||
SAT_ROT_RATE_MEKF,
|
||||
QUAT_MEKF,
|
||||
MEKF_STATUS,
|
||||
// Ctrl Values
|
||||
TGT_QUAT,
|
||||
ERROR_QUAT,
|
||||
ERROR_ANG,
|
||||
TGT_ROT_RATE,
|
||||
// Actuator Cmd
|
||||
RW_TARGET_TORQUE,
|
||||
RW_TARGET_SPEED,
|
||||
@ -109,7 +111,7 @@ 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 = 4;
|
||||
static constexpr uint8_t MEKF_SET_ENTRIES = 2;
|
||||
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 3;
|
||||
static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 4;
|
||||
static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
|
||||
|
||||
/**
|
||||
@ -238,6 +240,7 @@ class MekfData : public StaticLocalDataSet<MEKF_SET_ENTRIES> {
|
||||
|
||||
lp_vec_t<double, 4> quatMekf = lp_vec_t<double, 4>(sid.objectId, QUAT_MEKF, this);
|
||||
lp_vec_t<double, 3> satRotRateMekf = lp_vec_t<double, 3>(sid.objectId, SAT_ROT_RATE_MEKF, this);
|
||||
lp_var_t<uint8_t> mekfStatus = lp_var_t<uint8_t>(sid.objectId, MEKF_STATUS, this);
|
||||
|
||||
private:
|
||||
};
|
||||
@ -249,6 +252,7 @@ class CtrlValData : public StaticLocalDataSet<CTRL_VAL_SET_ENTRIES> {
|
||||
lp_vec_t<double, 4> tgtQuat = lp_vec_t<double, 4>(sid.objectId, TGT_QUAT, this);
|
||||
lp_vec_t<double, 4> errQuat = lp_vec_t<double, 4>(sid.objectId, ERROR_QUAT, this);
|
||||
lp_var_t<double> errAng = lp_var_t<double>(sid.objectId, ERROR_ANG, this);
|
||||
lp_vec_t<double, 3> tgtRotRate = lp_vec_t<double, 3>(sid.objectId, TGT_ROT_RATE, this);
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
@ -1 +1,2 @@
|
||||
target_sources(${LIB_EIVE_MISSION} PRIVATE GenericFactory.cpp scheduling.cpp)
|
||||
target_sources(${LIB_EIVE_MISSION} PRIVATE GenericFactory.cpp scheduling.cpp
|
||||
pollingSeqTables.cpp)
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include "pollingSequenceFactory.h"
|
||||
#include "pollingSeqTables.h"
|
||||
|
||||
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
||||
#include <fsfw/objectmanager/ObjectManagerIF.h>
|
||||
@ -659,7 +659,7 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
|
||||
DeviceHandlerIF::GET_READ);
|
||||
}
|
||||
|
||||
thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * 0.5, 0);
|
||||
thisSequence->addSlot(objects::SPI_RTD_COM_IF, length * config::acs::SCHED_BLOCK_RTD_PERIOD, 0);
|
||||
|
||||
return returnvalue::OK;
|
||||
}
|
||||
@ -108,8 +108,6 @@ class AcsBoardAssembly : public DualLaneAssemblyBase {
|
||||
static constexpr pcdu::Switches SWITCH_A = pcdu::Switches::PDU1_CH7_ACS_A_SIDE_3V3;
|
||||
static constexpr pcdu::Switches SWITCH_B = pcdu::Switches::PDU2_CH7_ACS_BOARD_SIDE_B_3V3;
|
||||
|
||||
bool tryingOtherSide = false;
|
||||
bool dualModeErrorSwitch = true;
|
||||
AcsBoardHelper helper;
|
||||
GpioIF* gpioIF = nullptr;
|
||||
|
||||
|
||||
@ -47,6 +47,11 @@ ReturnValue_t AcsSubsystem::initialize() {
|
||||
if (result != returnvalue::OK) {
|
||||
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
|
||||
}
|
||||
result = manager->subscribeToEvent(eventQueue->getId(),
|
||||
event::getEventId(acs::MEKF_INVALID_MODE_VIOLATION));
|
||||
if (result != returnvalue::OK) {
|
||||
sif::error << "AcsSubsystem: Subscribing for acs::MULTIPLE_RW_INVALID failed" << std::endl;
|
||||
}
|
||||
return Subsystem::initialize();
|
||||
}
|
||||
|
||||
@ -71,7 +76,8 @@ void AcsSubsystem::handleEventMessages() {
|
||||
}
|
||||
}
|
||||
if (event.getEvent() == acs::SAFE_RATE_RECOVERY ||
|
||||
event.getEvent() == acs::MULTIPLE_RW_INVALID) {
|
||||
event.getEvent() == acs::MULTIPLE_RW_INVALID ||
|
||||
event.getEvent() == acs::MEKF_INVALID_MODE_VIOLATION) {
|
||||
CommandMessage msg;
|
||||
ModeMessage::setCmdModeMessage(msg, acs::AcsMode::SAFE, 0);
|
||||
status = commandQueue->sendMessage(commandQueue->getId(), &msg);
|
||||
|
||||
@ -1,13 +1,36 @@
|
||||
#include "ComSubsystem.h"
|
||||
|
||||
#include <fsfw/events/EventManagerIF.h>
|
||||
#include <fsfw/ipc/MutexGuard.h>
|
||||
#include <fsfw/ipc/QueueFactory.h>
|
||||
#include <fsfw/serviceinterface/ServiceInterface.h>
|
||||
#include <linux/ipcore/PdecHandler.h>
|
||||
#include <mission/comDefs.h>
|
||||
#include <mission/config/comCfg.h>
|
||||
|
||||
#include "mission/config/comCfg.h"
|
||||
#include <utility>
|
||||
|
||||
ComSubsystem::ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences,
|
||||
uint32_t maxNumberOfTables)
|
||||
uint32_t maxNumberOfTables, uint32_t transmitterTimeout)
|
||||
: Subsystem(setObjectId, maxNumberOfSequences, maxNumberOfTables), paramHelper(this) {
|
||||
com::setCurrentDatarate(com::Datarate::LOW_RATE_MODULATION_BPSK);
|
||||
auto mqArgs = MqArgs(setObjectId, static_cast<void *>(this));
|
||||
eventQueue =
|
||||
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
|
||||
transmitterCountdown.setTimeout(transmitterTimeout);
|
||||
}
|
||||
|
||||
void ComSubsystem::performChildOperation() {
|
||||
readEventQueue();
|
||||
// Execute default rate sequence after transition has been completed
|
||||
if (rememberBitLock and not isInTransition) {
|
||||
startRxAndTxLowRateSeq();
|
||||
rememberBitLock = false;
|
||||
}
|
||||
if (countdownActive) {
|
||||
checkTransmitterCountdown();
|
||||
}
|
||||
Subsystem::performChildOperation();
|
||||
}
|
||||
|
||||
MessageQueueId_t ComSubsystem::getCommandQueue() const { return Subsystem::getCommandQueue(); }
|
||||
@ -27,6 +50,17 @@ ReturnValue_t ComSubsystem::getParameter(uint8_t domainId, uint8_t uniqueIdentif
|
||||
parameterWrapper->set(datarateCfg);
|
||||
com::setCurrentDatarate(static_cast<com::Datarate>(newVal));
|
||||
return returnvalue::OK;
|
||||
} else if ((domainId == 0) and
|
||||
(uniqueIdentifier == static_cast<uint8_t>(com::ParameterId::TRANSMITTER_TIMEOUT))) {
|
||||
uint32_t newVal = 0;
|
||||
ReturnValue_t result = newValues->getElement(&newVal);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
parameterWrapper->set(transmitterTimeout);
|
||||
transmitterTimeout = newVal;
|
||||
transmitterCountdown.setTimeout(transmitterTimeout);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
return returnvalue::OK;
|
||||
}
|
||||
@ -44,5 +78,121 @@ ReturnValue_t ComSubsystem::initialize() {
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
EventManagerIF *manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
|
||||
if (manager == nullptr) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "ComSubsystem::initialize: Invalid event manager" << std::endl;
|
||||
#endif
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
result = manager->registerListener(eventQueue->getId());
|
||||
if (result != returnvalue::OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "ComSubsystem::initialize: Failed to register Com Subsystem as event "
|
||||
"listener"
|
||||
<< std::endl;
|
||||
#endif
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
result = manager->subscribeToEventRange(eventQueue->getId(),
|
||||
event::getEventId(PdecHandler::CARRIER_LOCK),
|
||||
event::getEventId(PdecHandler::BIT_LOCK_PDEC));
|
||||
if (result != returnvalue::OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "ComSubsystem::initialize: Failed to subscribe to events from PDEC "
|
||||
"handler"
|
||||
<< std::endl;
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
return Subsystem::initialize();
|
||||
}
|
||||
|
||||
void ComSubsystem::startTransition(Mode_t mode, Submode_t submode) {
|
||||
// Depending on the submode the transmitter timeout is enabled or
|
||||
// disabled here
|
||||
if (mode == com::Submode::RX_ONLY) {
|
||||
transmitterCountdown.timeOut();
|
||||
countdownActive = false;
|
||||
} else if (isTxMode(mode)) {
|
||||
// Only start transmitter countdown if transmitter is not already on
|
||||
if (not isTxMode(this->mode)) {
|
||||
transmitterCountdown.resetTimer();
|
||||
countdownActive = true;
|
||||
}
|
||||
}
|
||||
Subsystem::startTransition(mode, submode);
|
||||
}
|
||||
|
||||
void ComSubsystem::readEventQueue() {
|
||||
EventMessage event;
|
||||
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
|
||||
result = eventQueue->receiveMessage(&event)) {
|
||||
switch (event.getMessageId()) {
|
||||
case EventMessage::EVENT_MESSAGE:
|
||||
handleEventMessage(&event);
|
||||
break;
|
||||
default:
|
||||
sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ComSubsystem::handleEventMessage(EventMessage *eventMessage) {
|
||||
Event event = eventMessage->getEvent();
|
||||
switch (event) {
|
||||
case PdecHandler::BIT_LOCK_PDEC: {
|
||||
handleBitLockEvent();
|
||||
break;
|
||||
}
|
||||
case PdecHandler::CARRIER_LOCK: {
|
||||
handleCarrierLockEvent();
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::debug << "ComSubsystem::handleEvent: Did not subscribe to this event" << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ComSubsystem::handleBitLockEvent() {
|
||||
if (isTxMode(mode)) {
|
||||
// Tx already on
|
||||
return;
|
||||
}
|
||||
if (isInTransition) {
|
||||
rememberBitLock = true;
|
||||
return;
|
||||
}
|
||||
startRxAndTxLowRateSeq();
|
||||
}
|
||||
|
||||
void ComSubsystem::handleCarrierLockEvent() {
|
||||
if (!enableTxWhenCarrierLock) {
|
||||
return;
|
||||
}
|
||||
startRxAndTxLowRateSeq();
|
||||
}
|
||||
|
||||
void ComSubsystem::startRxAndTxLowRateSeq() {
|
||||
// Turns transmitter on
|
||||
startTransition(com::Submode::RX_AND_TX_LOW_DATARATE, SUBMODE_NONE);
|
||||
}
|
||||
|
||||
void ComSubsystem::checkTransmitterCountdown() {
|
||||
if (transmitterCountdown.hasTimedOut()) {
|
||||
startTransition(com::Submode::RX_ONLY, SUBMODE_NONE);
|
||||
countdownActive = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool ComSubsystem::isTxMode(Mode_t mode) {
|
||||
if ((mode == com::Submode::RX_AND_TX_DEFAULT_DATARATE) ||
|
||||
(mode == com::Submode::RX_AND_TX_LOW_DATARATE) ||
|
||||
(mode == com::Submode::RX_AND_TX_HIGH_DATARATE)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#ifndef MISSION_SYSTEM_COMSUBSYSTEM_H_
|
||||
#define MISSION_SYSTEM_COMSUBSYSTEM_H_
|
||||
|
||||
#include <fsfw/events/EventMessage.h>
|
||||
#include <fsfw/parameters/HasParametersIF.h>
|
||||
#include <fsfw/parameters/ParameterHelper.h>
|
||||
#include <fsfw/subsystem/Subsystem.h>
|
||||
@ -9,21 +10,71 @@
|
||||
|
||||
class ComSubsystem : public Subsystem, public ReceivesParameterMessagesIF {
|
||||
public:
|
||||
ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables);
|
||||
/**
|
||||
* @brief Constructor
|
||||
*
|
||||
* @param setObjectId
|
||||
* @param maxNumberOfSequences
|
||||
* @param maxNumberOfTables
|
||||
* @param transmitterTimeout Maximum time the transmitter of the syrlinks
|
||||
* will be
|
||||
* enabled
|
||||
*/
|
||||
ComSubsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables,
|
||||
uint32_t transmitterTimeout);
|
||||
virtual ~ComSubsystem() = default;
|
||||
|
||||
MessageQueueId_t getCommandQueue() const override;
|
||||
ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
|
||||
ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues,
|
||||
uint16_t startAtIndex) override;
|
||||
virtual void performChildOperation() override;
|
||||
|
||||
private:
|
||||
static const Mode_t INITIAL_MODE = 0;
|
||||
|
||||
ReturnValue_t handleCommandMessage(CommandMessage *message) override;
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
|
||||
void startTransition(Mode_t mode, Submode_t submode) override;
|
||||
|
||||
void readEventQueue();
|
||||
void handleEventMessage(EventMessage *eventMessage);
|
||||
void handleBitLockEvent();
|
||||
void handleCarrierLockEvent();
|
||||
void checkTransmitterCountdown();
|
||||
/**
|
||||
* @brief Enables transmitter in low rate mode
|
||||
*/
|
||||
void startRxAndTxLowRateSeq();
|
||||
|
||||
/**
|
||||
* @brief Returns true if mode is a mode where the transmitter is on
|
||||
*/
|
||||
bool isTxMode(Mode_t mode);
|
||||
|
||||
uint8_t datarateCfg = static_cast<uint8_t>(com::Datarate::LOW_RATE_MODULATION_BPSK);
|
||||
// Maximum time after which the transmitter will be turned of. This is a
|
||||
// protection mechanism due prevent the syrlinks from overheating
|
||||
uint32_t transmitterTimeout = 0;
|
||||
ParameterHelper paramHelper;
|
||||
|
||||
MessageQueueIF *eventQueue = nullptr;
|
||||
|
||||
bool enableTxWhenCarrierLock = false;
|
||||
|
||||
// Countdown will be started as soon as the transmitter was enabled
|
||||
Countdown transmitterCountdown;
|
||||
|
||||
// Transmitter countdown only active when sysrlinks transmitter is on (modes:
|
||||
// rx and tx low rate, rx and tx high rate, rx and tx default rate)
|
||||
bool countdownActive = false;
|
||||
|
||||
// True when bit lock occurred while COM subsystem is in a transition. This
|
||||
// variable is used to remember the bit lock and execute the default rate
|
||||
// sequence after the active transition has been completed
|
||||
bool rememberBitLock = false;
|
||||
};
|
||||
|
||||
#endif /* MISSION_SYSTEM_COMSUBSYSTEM_H_ */
|
||||
|
||||
@ -10,6 +10,7 @@ enum class HandlerState { SWITCH_PENDING, IDLE };
|
||||
|
||||
class Stack5VHandler {
|
||||
public:
|
||||
//! [EXPORT] : [SKIP]
|
||||
static constexpr ReturnValue_t BUSY = returnvalue::makeCode(1, 0);
|
||||
Stack5VHandler(PowerSwitchIF& switcher);
|
||||
|
||||
|
||||
@ -50,8 +50,6 @@ class SusAssembly : public DualLaneAssemblyBase {
|
||||
|
||||
SusAssHelper helper;
|
||||
PowerSwitchIF* pwrSwitcher = nullptr;
|
||||
bool tryingOtherSide = false;
|
||||
bool dualModeErrorSwitch = true;
|
||||
ReturnValue_t initialize() override;
|
||||
|
||||
// AssemblyBase overrides
|
||||
|
||||
@ -1,6 +1,5 @@
|
||||
#include "comModeTree.h"
|
||||
|
||||
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
||||
#include <fsfw/modes/HasModesIF.h>
|
||||
#include <fsfw/returnvalues/returnvalue.h>
|
||||
#include <fsfw/subsystem/Subsystem.h>
|
||||
@ -11,7 +10,8 @@
|
||||
|
||||
const auto check = subsystem::checkInsert;
|
||||
|
||||
ComSubsystem satsystem::com::SUBSYSTEM = ComSubsystem(objects::COM_SUBSYSTEM, 12, 24);
|
||||
ComSubsystem satsystem::com::SUBSYSTEM =
|
||||
ComSubsystem(objects::COM_SUBSYSTEM, 12, 24, TRANSMITTER_TIMEOUT);
|
||||
|
||||
static const auto OFF = HasModesIF::MODE_OFF;
|
||||
static const auto ON = HasModesIF::MODE_ON;
|
||||
@ -19,39 +19,48 @@ static const auto NML = DeviceHandlerIF::MODE_NORMAL;
|
||||
|
||||
auto COM_SEQUENCE_RX_ONLY =
|
||||
std::make_pair(::com::Submode::RX_ONLY, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_ONLY_TGT =
|
||||
std::make_pair((::com::Submode::RX_ONLY << 24) | 1, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_ONLY_TRANS_0 =
|
||||
std::make_pair((::com::Submode::RX_ONLY << 24) | 2, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_ONLY_TRANS_1 =
|
||||
std::make_pair((::com::Submode::RX_ONLY << 24) | 3, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_ONLY_TGT = std::make_pair(
|
||||
static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 1, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_ONLY_TRANS_0 = std::make_pair(
|
||||
static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 2, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_ONLY_TRANS_1 = std::make_pair(
|
||||
static_cast<uint32_t>(::com::Submode::RX_ONLY << 24) | 3, FixedArrayList<ModeListEntry, 3>());
|
||||
|
||||
auto COM_SEQUENCE_RX_AND_TX_LOW_RATE =
|
||||
std::make_pair(::com::Submode::RX_AND_TX_LOW_DATARATE, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_LOW_RATE_TGT = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0 = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1 = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_LOW_RATE_TGT =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 1,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_0 =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 2,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_LOW_RATE_TRANS_1 =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_LOW_DATARATE << 24) | 3,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
|
||||
auto COM_SEQUENCE_RX_AND_TX_HIGH_RATE =
|
||||
std::make_pair(::com::Submode::RX_AND_TX_HIGH_DATARATE, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TGT = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0 = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1 = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TGT =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 1,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_0 =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 2,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_HIGH_RATE_TRANS_1 =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_HIGH_DATARATE << 24) | 3,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
|
||||
auto COM_SEQUENCE_RX_AND_TX_DEFAULT_RATE =
|
||||
std::make_pair(::com::Submode::RX_AND_TX_DEFAULT_DATARATE, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0 = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1 = std::make_pair(
|
||||
(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3, FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TGT =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 1,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_0 =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 2,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
auto COM_TABLE_RX_AND_TX_DEFAULT_RATE_TRANS_1 =
|
||||
std::make_pair(static_cast<uint32_t>(::com::Submode::RX_AND_TX_DEFAULT_DATARATE << 24) | 3,
|
||||
FixedArrayList<ModeListEntry, 3>());
|
||||
|
||||
namespace {
|
||||
|
||||
@ -68,7 +77,7 @@ Subsystem& satsystem::com::init() {
|
||||
buildTxAndRxLowRateSequence(SUBSYSTEM, entry);
|
||||
buildTxAndRxHighRateSequence(SUBSYSTEM, entry);
|
||||
buildTxAndRxDefaultRateSequence(SUBSYSTEM, entry);
|
||||
SUBSYSTEM.setInitialMode(NML, ::com::Submode::RX_ONLY);
|
||||
SUBSYSTEM.setInitialMode(COM_SEQUENCE_RX_ONLY.first);
|
||||
return SUBSYSTEM;
|
||||
}
|
||||
|
||||
@ -95,7 +104,7 @@ void buildRxOnlySequence(Subsystem& ss, ModeListEntry& eh) {
|
||||
|
||||
// Build RX Only table. We could track the state of the CCSDS IP core handler
|
||||
// as well but I do not think this is necessary because enabling that should
|
||||
// not intefere with the Syrlinks Handler.
|
||||
// not interfere with the Syrlinks Handler.
|
||||
iht(objects::SYRLINKS_HANDLER, NML, ::com::Submode::RX_ONLY, COM_TABLE_RX_ONLY_TGT.second);
|
||||
check(ss.addTable(TableEntry(COM_TABLE_RX_ONLY_TGT.first, &COM_TABLE_RX_ONLY_TGT.second)), ctxc);
|
||||
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#ifndef MISSION_SYSTEM_TREE_COMMODETREE_H_
|
||||
#define MISSION_SYSTEM_TREE_COMMODETREE_H_
|
||||
|
||||
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
||||
#include <mission/system/objects/ComSubsystem.h>
|
||||
|
||||
namespace satsystem {
|
||||
@ -8,6 +9,11 @@ namespace satsystem {
|
||||
namespace com {
|
||||
extern ComSubsystem SUBSYSTEM;
|
||||
|
||||
// The syrlinks must not transmitting longer then 15 minutes otherwise the
|
||||
// transceiver might be damaged due to overheating
|
||||
// 15 minutes in milliseconds
|
||||
static const uint32_t TRANSMITTER_TIMEOUT = 900000;
|
||||
|
||||
Subsystem& init();
|
||||
} // namespace com
|
||||
|
||||
|
||||
@ -7,6 +7,7 @@
|
||||
#include "acsModeTree.h"
|
||||
#include "comModeTree.h"
|
||||
#include "eive/objects.h"
|
||||
#include "mission/comDefs.h"
|
||||
#include "payloadModeTree.h"
|
||||
#include "tcsModeTree.h"
|
||||
#include "util.h"
|
||||
@ -85,6 +86,7 @@ void buildSafeSequence(Subsystem& ss, ModeListEntry& eh) {
|
||||
// Build SAFE transition 0. Two transitions to reduce number of consecutive events and because
|
||||
// consecutive commanding of TCS and ACS can lead to SPI issues.
|
||||
iht(objects::TCS_SUBSYSTEM, NML, 0, EIVE_TABLE_SAFE_TRANS_0.second);
|
||||
iht(objects::COM_SUBSYSTEM, com::RX_ONLY, 0, EIVE_TABLE_SAFE_TRANS_0.second);
|
||||
check(ss.addTable(TableEntry(EIVE_TABLE_SAFE_TRANS_0.first, &EIVE_TABLE_SAFE_TRANS_0.second)),
|
||||
ctxc);
|
||||
|
||||
|
||||
@ -1,12 +1,10 @@
|
||||
#include "CcsdsIpCoreHandler.h"
|
||||
|
||||
#include <fsfw/subsystem/helper.h>
|
||||
#include <linux/ipcore/PdecHandler.h>
|
||||
#include <linux/ipcore/PtmeConfig.h>
|
||||
#include <mission/config/comCfg.h>
|
||||
|
||||
#include "eive/definitions.h"
|
||||
#include "fsfw/events/EventManagerIF.h"
|
||||
#include "fsfw/ipc/QueueFactory.h"
|
||||
#include "fsfw/objectmanager/ObjectManager.h"
|
||||
#include "fsfw/serialize/SerializeAdapter.h"
|
||||
@ -16,8 +14,7 @@
|
||||
|
||||
CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
|
||||
object_id_t tcDestination, PtmeConfig* ptmeConfig,
|
||||
GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData,
|
||||
uint32_t transmitterTimeout)
|
||||
GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData)
|
||||
: SystemObject(objectId),
|
||||
ptmeId(ptmeId),
|
||||
tcDestination(tcDestination),
|
||||
@ -27,8 +24,7 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
|
||||
ptmeConfig(ptmeConfig),
|
||||
gpioIF(gpioIF),
|
||||
enTxClock(enTxClock),
|
||||
enTxData(enTxData),
|
||||
transmitterTimeout(transmitterTimeout) {
|
||||
enTxData(enTxData) {
|
||||
commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
|
||||
auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
|
||||
eventQueue =
|
||||
@ -38,11 +34,9 @@ CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
|
||||
CcsdsIpCoreHandler::~CcsdsIpCoreHandler() {}
|
||||
|
||||
ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) {
|
||||
checkEvents();
|
||||
readCommandQueue();
|
||||
handleTelemetry();
|
||||
handleTelecommands();
|
||||
checkTxTimer();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
@ -98,46 +92,11 @@ ReturnValue_t CcsdsIpCoreHandler::initialize() {
|
||||
iter->second->setPtmeObject(ptme);
|
||||
}
|
||||
|
||||
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
|
||||
if (manager == nullptr) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "CcsdsHandler::initialize: Invalid event manager" << std::endl;
|
||||
#endif
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
result = manager->registerListener(eventQueue->getId());
|
||||
if (result != returnvalue::OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "CcsdsHandler::initialize: Failed to register CCSDS handler as event "
|
||||
"listener"
|
||||
<< std::endl;
|
||||
#endif
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
;
|
||||
}
|
||||
result = manager->subscribeToEventRange(eventQueue->getId(),
|
||||
event::getEventId(PdecHandler::CARRIER_LOCK),
|
||||
event::getEventId(PdecHandler::BIT_LOCK_PDEC));
|
||||
if (result != returnvalue::OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "CcsdsHandler::initialize: Failed to subscribe to events from PDEC "
|
||||
"handler"
|
||||
<< std::endl;
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
result = ptmeConfig->initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
if (result != returnvalue::OK) {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
transmitterCountdown.setTimeout(transmitterTimeout);
|
||||
transmitterCountdown.timeOut();
|
||||
|
||||
#if OBSW_SYRLINKS_SIMULATED == 1
|
||||
// Update data on rising edge
|
||||
ptmeConfig->invertTxClock(false);
|
||||
@ -286,54 +245,6 @@ ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueu
|
||||
return EXECUTION_FINISHED;
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::checkEvents() {
|
||||
EventMessage event;
|
||||
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
|
||||
result = eventQueue->receiveMessage(&event)) {
|
||||
switch (event.getMessageId()) {
|
||||
case EventMessage::EVENT_MESSAGE:
|
||||
handleEvent(&event);
|
||||
break;
|
||||
default:
|
||||
sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::handleEvent(EventMessage* eventMessage) {
|
||||
Event event = eventMessage->getEvent();
|
||||
switch (event) {
|
||||
case PdecHandler::BIT_LOCK_PDEC: {
|
||||
handleBitLockEvent();
|
||||
break;
|
||||
}
|
||||
case PdecHandler::CARRIER_LOCK: {
|
||||
handleCarrierLockEvent();
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::debug << "CcsdsHandler::handleEvent: Did not subscribe to this event" << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::handleBitLockEvent() {
|
||||
if (transmitterCountdown.isBusy()) {
|
||||
// Transmitter already enabled
|
||||
return;
|
||||
}
|
||||
enableTransmit();
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::handleCarrierLockEvent() {
|
||||
if (!enableTxWhenCarrierLock) {
|
||||
return;
|
||||
}
|
||||
enableTransmit();
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::forwardLinkstate() {
|
||||
VirtualChannelMapIter iter;
|
||||
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
|
||||
@ -342,27 +253,12 @@ void CcsdsIpCoreHandler::forwardLinkstate() {
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::enableTransmit() {
|
||||
if (transmitterCountdown.isBusy()) {
|
||||
// Transmitter already enabled
|
||||
return;
|
||||
}
|
||||
#ifndef TE0720_1CFA
|
||||
gpioIF->pullHigh(enTxClock);
|
||||
gpioIF->pullHigh(enTxData);
|
||||
#endif
|
||||
linkState = UP;
|
||||
forwardLinkstate();
|
||||
transmitterCountdown.resetTimer();
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::checkTxTimer() {
|
||||
if (linkState == DOWN) {
|
||||
return;
|
||||
}
|
||||
if (transmitterCountdown.hasTimedOut()) {
|
||||
disableTransmit();
|
||||
// TODO: set mode to off (move timer to subsystem)
|
||||
}
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::getMode(Mode_t* mode, Submode_t* submode) {
|
||||
@ -431,7 +327,6 @@ void CcsdsIpCoreHandler::disableTransmit() {
|
||||
#endif
|
||||
linkState = DOWN;
|
||||
forwardLinkstate();
|
||||
transmitterCountdown.timeOut();
|
||||
}
|
||||
|
||||
const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; }
|
||||
|
||||
@ -59,8 +59,7 @@ class CcsdsIpCoreHandler : public SystemObject,
|
||||
* @param enTxData GPIO ID of RS485 tx data enable
|
||||
*/
|
||||
CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId, object_id_t tcDestination,
|
||||
PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData,
|
||||
uint32_t transmitterTimeout = 900000);
|
||||
PtmeConfig* ptmeConfig, GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData);
|
||||
|
||||
~CcsdsIpCoreHandler();
|
||||
|
||||
@ -154,29 +153,16 @@ class CcsdsIpCoreHandler : public SystemObject,
|
||||
PtmeConfig* ptmeConfig = nullptr;
|
||||
|
||||
GpioIF* gpioIF = nullptr;
|
||||
// GPIO to enable RS485 transceiver for TX clock
|
||||
gpioId_t enTxClock = gpio::NO_GPIO;
|
||||
// GPIO to enable RS485 transceiver for TX data signal
|
||||
gpioId_t enTxData = gpio::NO_GPIO;
|
||||
|
||||
// Syrlinks must not be transmitting more than 15 minutes (according to datasheet)
|
||||
// Value initialized by constructor argument
|
||||
const uint32_t transmitterTimeout = 0;
|
||||
|
||||
// Countdown to disable transmitter after 15 minutes
|
||||
Countdown transmitterCountdown;
|
||||
|
||||
// When true transmitting is started as soon as carrier lock has been detected
|
||||
bool enableTxWhenCarrierLock = false;
|
||||
|
||||
bool linkState = DOWN;
|
||||
|
||||
void readCommandQueue(void);
|
||||
void handleTelemetry();
|
||||
void handleTelecommands();
|
||||
void checkEvents();
|
||||
void handleEvent(EventMessage* eventMessage);
|
||||
|
||||
void handleBitLockEvent();
|
||||
void handleCarrierLockEvent();
|
||||
|
||||
/**
|
||||
* @brief Forward link state to virtual channels.
|
||||
@ -188,12 +174,6 @@ class CcsdsIpCoreHandler : public SystemObject,
|
||||
*/
|
||||
void enableTransmit();
|
||||
|
||||
/**
|
||||
* @brief Checks Tx timer for timeout and disables RS485 tx clock and tx data in case
|
||||
* timer has expired.
|
||||
*/
|
||||
void checkTxTimer();
|
||||
|
||||
/**
|
||||
* @brief Disables the transmitter by pulling the enable tx clock and tx data pin of the
|
||||
* RS485 transceiver chips to high.
|
||||
|
||||
2
tmtc
2
tmtc
@ -1 +1 @@
|
||||
Subproject commit 841780593ebe35ce01ea95dc5e21b78237f1d861
|
||||
Subproject commit 24f0d8e1a6a8ea1323623932e699326214c78159
|
||||
Loading…
Reference in New Issue
Block a user