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

ACS Scheduling and Smaller ACS Fixes #380

Merged
muellerr merged 8 commits from eggert/acs-scheduling-and-smaller-stuff into develop 2023-02-15 20:23:16 +01:00
Conversation 0 Commits 8 Files Changed 7 +113 -112
7 changed files with 113 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
CHANGELOG.md
View File

@ -17,6 +17,15 @@ change warranting a new major release:
# [unreleased] # [unreleased]
## Changed
- Igrf13 model vector now outputs as uT instead of nT
- Changed timings for `AcsPst`
- Added values for MGM sensor fusion
## Fixed
- Fixed values for GYR sensor fusion
# [v1.27.2] 2023-02-14 # [v1.27.2] 2023-02-14
Reaction Wheel handling was determined to be (quasi) broken and needs to be fixed in future release Reaction Wheel handling was determined to be (quasi) broken and needs to be fixed in future release

15
common/config/eive/definitions.h
View File

@ -57,14 +57,17 @@ static constexpr uint32_t MAX_STORED_CMDS_TCP = 150;
namespace acs { namespace acs {
static constexpr uint32_t SCHED_BLOCK_1_SENSORS_MS = 15; static constexpr uint32_t SCHED_BLOCK_1_SUS_READ_MS = 15;
static constexpr uint32_t SCHED_BLOCK_2_ACS_CTRL_MS = 40; static constexpr uint32_t SCHED_BLOCK_2_SENSOR_READ_MS = 30;
static constexpr uint32_t SCHED_BLOCK_3_ACTUATOR_MS = 45; static constexpr uint32_t SCHED_BLOCK_3_ACS_CTRL_MS = 45;
static constexpr uint32_t SCHED_BLOCK_4_ACTUATOR_MS = 50;
// 15 ms for FM // 15 ms for FM
static constexpr float SCHED_BLOCK_1_PERIOD = static_cast<float>(SCHED_BLOCK_1_SENSORS_MS) / 400.0; static constexpr float SCHED_BLOCK_1_PERIOD = static_cast<float>(SCHED_BLOCK_1_SUS_READ_MS) / 400.0;
static constexpr float SCHED_BLOCK_2_PERIOD = static_cast<float>(SCHED_BLOCK_2_ACS_CTRL_MS) / 400.0; static constexpr float SCHED_BLOCK_2_PERIOD =
static constexpr float SCHED_BLOCK_3_PERIOD = static_cast<float>(SCHED_BLOCK_3_ACTUATOR_MS) / 400.0; static_cast<float>(SCHED_BLOCK_2_SENSOR_READ_MS) / 400.0;
static constexpr float SCHED_BLOCK_3_PERIOD = static_cast<float>(SCHED_BLOCK_3_ACS_CTRL_MS) / 400.0;
static constexpr float SCHED_BLOCK_4_PERIOD = static_cast<float>(SCHED_BLOCK_4_ACTUATOR_MS) / 400.0;
} // namespace acs } // namespace acs

172
linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
View File

@ -193,102 +193,102 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
if (cfg.scheduleAcsBoard) { if (cfg.scheduleAcsBoard) {
if (enableAside) { if (enableAside) {
// A side // A side
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_0_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_0_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
} }
if (enableBside) { if (enableBside) {
// B side // B side
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::MGM_2_LIS3_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER, thisSequence->addSlot(objects::MGM_3_RM3100_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_WRITE); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::SEND_READ); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER, thisSequence->addSlot(objects::GYRO_2_ADIS_HANDLER,
length * config::acs::SCHED_BLOCK_1_PERIOD, DeviceHandlerIF::GET_READ); length * config::acs::SCHED_BLOCK_2_PERIOD, DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_1_PERIOD, thisSequence->addSlot(objects::GYRO_3_L3G_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
} }
} }
@ -616,19 +616,19 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
} }
thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_2_PERIOD, 0); thisSequence->addSlot(objects::ACS_CONTROLLER, length * config::acs::SCHED_BLOCK_3_PERIOD, 0);
if (cfg.scheduleImtq) { if (cfg.scheduleImtq) {
// This is the torquing cycle. // This is the torquing cycle.
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::IMTQ_HANDLER, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
} }
@ -711,94 +711,94 @@ ReturnValue_t pst::pstTcsAndAcs(FixedTimeslotTaskIF *thisSequence, AcsPstCfg cfg
if (cfg.scheduleRws) { if (cfg.scheduleRws) {
// this is the torquing cycle // this is the torquing cycle
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::PERFORM_OPERATION); DeviceHandlerIF::PERFORM_OPERATION);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_WRITE); DeviceHandlerIF::SEND_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_WRITE); DeviceHandlerIF::GET_WRITE);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::SEND_READ); DeviceHandlerIF::SEND_READ);
thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW1, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW2, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW3, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_2_PERIOD, thisSequence->addSlot(objects::RW4, length * config::acs::SCHED_BLOCK_4_PERIOD,
DeviceHandlerIF::GET_READ); DeviceHandlerIF::GET_READ);
} }

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

@ -73,9 +73,9 @@ class AcsParameters : public HasParametersIF {
{-0.007534, 1.253879, 0.006812}, {-0.007534, 1.253879, 0.006812},
{-0.037072, 0.006812, 1.313158}}; {-0.037072, 0.006812, 1.313158}};
float mgm02variance[3] = {1, 1, 1}; float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)};
float mgm13variance[3] = {1, 1, 1}; float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)};
float mgm4variance[3] = {1, 1, 1}; float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)};
} mgmHandlingParameters; } mgmHandlingParameters;
struct SusHandlingParameters { struct SusHandlingParameters {
@ -779,9 +779,9 @@ class AcsParameters : public HasParametersIF {
/* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
* assumed to be equal for the same class of sensors */ * assumed to be equal for the same class of sensors */
float gyr02variance[3] = {pow(3.0e-3 * sqrt(2), 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
pow(3.0e-3 * sqrt(2), 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms pow(3.0e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
pow(4.3e-3 * sqrt(2), 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms pow(4.3e-3, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)}; float gyr13variance[3] = {pow(11e-3, 2), pow(11e-3, 2), pow(11e-3, 2)};
uint8_t preferAdis = true; uint8_t preferAdis = true;
} gyrHandlingParameters; } gyrHandlingParameters;

7
mission/controller/acs/ActuatorCmd.cpp
View File

@ -1,10 +1,3 @@
/*
* ActuatorCmd.cpp
*
* Created on: 4 Aug 2022
* Author: Robin Marquardt
*/
#include "ActuatorCmd.h" #include "ActuatorCmd.h"
#include <fsfw/globalfunctions/constants.h> #include <fsfw/globalfunctions/constants.h>

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

@ -20,6 +20,7 @@ Igrf13Model::~Igrf13Model() {}
void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude, void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
const double altitude, timeval timeOfMagMeasurement, const double altitude, timeval timeOfMagMeasurement,
double* magFieldModelInertial) { double* magFieldModelInertial) {
double magFieldModel[3] = {0, 0, 0};
double phi = longitude, theta = gcLatitude; // geocentric double phi = longitude, theta = gcLatitude; // geocentric
/* Here is the co-latitude needed*/ /* Here is the co-latitude needed*/
theta -= 90 * PI / 180; theta -= 90 * PI / 180;
@ -100,12 +101,8 @@ void Igrf13Model::magFieldComp(const double longitude, const double gcLatitude,
magFieldModelInertial[2] = magFieldModelInertial[2] =
magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude); magFieldModel[0] * sin(gcLatitude) - magFieldModel[1] * cos(gcLatitude);
double normVecMagFieldInert[3] = {0, 0, 0}; // convert nT to uT
VectorOperations<double>::normalize(magFieldModelInertial, normVecMagFieldInert, 3); VectorOperations<double>::mulScalar(magFieldModelInertial, 1e-3, magFieldModelInertial, 3);
magFieldModel[0] = 0;
magFieldModel[1] = 0;
magFieldModel[2] = 0;
} }
void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) { void Igrf13Model::updateCoeffGH(timeval timeOfMagMeasurement) {

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

@ -47,7 +47,6 @@ class Igrf13Model /*:public HasParametersIF*/ {
// Coefficient wary over year, could be updated sometimes. // Coefficient wary over year, could be updated sometimes.
void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV) void updateCoeffGH(timeval timeOfMagMeasurement); // Secular variation (SV)
double magFieldModel[3];
void schmidtNormalization(); void schmidtNormalization();
private: private: