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

Merge branch 'develop' into bugfixes_code_review_msg_handling
All checks were successful
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
Details

Browse Source
This commit is contained in:
Steffen Gaisser
2023-02-21 11:29:03 +01:00
parent 9a9574369a 257efd4974
commit 0f5e65e5ce
48 changed files with 3321 additions and 2407 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
mission/controller/AcsController.h
View File

@ -5,6 +5,7 @@
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/parameters/ParameterHelper.h>
#include <fsfw/parameters/ReceivesParameterMessagesIF.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include "acs/ActuatorCmd.h"
@ -19,7 +20,6 @@
#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
#include "mission/trace.h"
class AcsController : public ExtendedControllerBase, public ReceivesParameterMessagesIF {
@ -84,7 +84,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
ACS::SensorValues sensorValues;
/* ACS Actuation Datasets */
IMTQ::DipoleActuationSet dipoleSet = IMTQ::DipoleActuationSet(objects::IMTQ_HANDLER);
imtq::DipoleActuationSet dipoleSet = imtq::DipoleActuationSet(objects::IMTQ_HANDLER);
rws::RwSpeedActuationSet rw1SpeedSet = rws::RwSpeedActuationSet(objects::RW1);
rws::RwSpeedActuationSet rw2SpeedSet = rws::RwSpeedActuationSet(objects::RW2);
rws::RwSpeedActuationSet rw3SpeedSet = rws::RwSpeedActuationSet(objects::RW3);

183
mission/controller/ThermalController.cpp
View File

@ -10,7 +10,7 @@
#include <mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h>
#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/payloadPcduDefinitions.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include <objects/systemObjectList.h>
@ -21,6 +21,7 @@ ThermalController::ThermalController(object_id_t objectId, HeaterHandler& heater
sensorTemperatures(this),
susTemperatures(this),
deviceTemperatures(this),
heaterInfo(this),
imtqThermalSet(objects::IMTQ_HANDLER, ThermalStateCfg()),
max31865Set0(objects::RTD_0_IC3_PLOC_HEATSPREADER,
EiveMax31855::RtdCommands::EXCHANGE_SET_ID),
@ -112,108 +113,88 @@ void ThermalController::performControlOperation() {
deviceTemperatures.commit();
}
std::array<HeaterHandler::SwitchState, 8> heaterStates;
heaterHandler.getAllSwitchStates(heaterStates);
{
PoolReadGuard pg(&heaterInfo);
std::memcpy(heaterInfo.heaterSwitchState.value, heaterStates.data(), 8);
{
PoolReadGuard pg2(&currentVecPdu2);
if (pg.getReadResult() == returnvalue::OK and pg2.getReadResult() == returnvalue::OK) {
heaterInfo.heaterCurrent.value = currentVecPdu2.value[PDU2::Channels::TCS_HEATER_IN];
}
}
}
// performThermalModuleCtrl();
}
ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) {
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_HEATSPREADER,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLOC_MISSIONBOARD,
new PoolEntry<float>({1.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_4K_CAMERA,
new PoolEntry<float>({2.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DAC_HEATSPREADER,
new PoolEntry<float>({3.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_STARTRACKER,
new PoolEntry<float>({4.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_RW1, new PoolEntry<float>({5.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_DRO, new PoolEntry<float>({6.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_SCEX, new PoolEntry<float>({7.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_X8, new PoolEntry<float>({8.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_HPA, new PoolEntry<float>({9.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TX_MODUL,
new PoolEntry<float>({10.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MPA, new PoolEntry<float>({11.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_ACU, new PoolEntry<float>({12.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_PLPCDU_HEATSPREADER,
new PoolEntry<float>({13.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TCS_BOARD,
new PoolEntry<float>({14.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_MAGNETTORQUER,
new PoolEntry<float>({15.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
localDataPoolMap.emplace(thermalControllerDefinitions::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);
localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_HEATSPREADER, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_PLOC_MISSIONBOARD, new PoolEntry<float>({1.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_4K_CAMERA, new PoolEntry<float>({2.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_DAC_HEATSPREADER, new PoolEntry<float>({3.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_STARTRACKER, new PoolEntry<float>({4.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_RW1, new PoolEntry<float>({5.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_DRO, new PoolEntry<float>({6.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_SCEX, new PoolEntry<float>({7.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_X8, new PoolEntry<float>({8.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_HPA, new PoolEntry<float>({9.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_TX_MODUL, new PoolEntry<float>({10.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_MPA, new PoolEntry<float>({11.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_ACU, new PoolEntry<float>({12.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_PLPCDU_HEATSPREADER, new PoolEntry<float>({13.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_TCS_BOARD, new PoolEntry<float>({14.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_MAGNETTORQUER, new PoolEntry<float>({15.0}));
localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_0, &tmp1075Tcs0);
localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_TCS_1, &tmp1075Tcs1);
localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_0, &tmp1075PlPcdu0);
localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_PLPCDU_1, &tmp1075PlPcdu1);
localDataPoolMap.emplace(tcsCtrl::SENSOR_TMP1075_IF_BOARD, &tmp1075IfBrd);
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_0_N_LOC_XFYFZM_PT_XF,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_6_R_LOC_XFYBZM_PT_XF,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_1_N_LOC_XBYFZM_PT_XB,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_7_R_LOC_XBYBZM_PT_XB,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_2_N_LOC_XFYBZB_PT_YB,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_8_R_LOC_XBYBZB_PT_YB,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_3_N_LOC_XFYBZF_PT_YF,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_9_R_LOC_XBYBZB_PT_YF,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_4_N_LOC_XMYFZF_PT_ZF,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_10_N_LOC_XMYBZF_PT_ZF,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_5_N_LOC_XFYMZB_PT_ZB,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::SUS_11_R_LOC_XBYMZB_PT_ZB,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_0_N_LOC_XFYFZM_PT_XF, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_6_R_LOC_XFYBZM_PT_XF, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_1_N_LOC_XBYFZM_PT_XB, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_7_R_LOC_XBYBZM_PT_XB, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_2_N_LOC_XFYBZB_PT_YB, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_8_R_LOC_XBYBZB_PT_YB, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_3_N_LOC_XFYBZF_PT_YF, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_9_R_LOC_XBYBZB_PT_YF, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_4_N_LOC_XMYFZF_PT_ZF, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_10_N_LOC_XMYBZF_PT_ZF, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_5_N_LOC_XFYMZB_PT_ZB, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::SUS_11_R_LOC_XBYMZB_PT_ZB, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::COMPONENT_RW, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::COMPONENT_RW, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_Q7S, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_1,
new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_2,
new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_3,
new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::BATTERY_TEMP_4,
new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW1, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW2, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW3, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_RW4, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_STAR_TRACKER,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_SYRLINKS_POWER_AMPLIFIER,
new PoolEntry<float>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_SYRLINKS_BASEBAND_BOARD,
new PoolEntry<float>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_ACU, new PoolEntry<float>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_PDU1, new PoolEntry<float>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_PDU2, new PoolEntry<float>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_0_SIDE_A,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_1_SIDE_A,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_2_SIDE_B,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_GYRO_3_SIDE_B,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGM_0_SIDE_A,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_MGM_2_SIDE_B,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(thermalControllerDefinitions::TEMP_ADC_PAYLOAD_PCDU,
new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_Q7S, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_1, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_2, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_3, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(tcsCtrl::BATTERY_TEMP_4, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_RW1, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_RW2, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_RW3, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_RW4, new PoolEntry<int32_t>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_STAR_TRACKER, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_POWER_AMPLIFIER, new PoolEntry<float>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_SYRLINKS_BASEBAND_BOARD, new PoolEntry<float>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_MGT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_ACU, new PoolEntry<float>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_PDU1, new PoolEntry<float>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_PDU2, new PoolEntry<float>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_1_P60DOCK, new PoolEntry<float>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_2_P60DOCK, new PoolEntry<float>({0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_0_SIDE_A, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_1_SIDE_A, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_2_SIDE_B, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_GYRO_3_SIDE_B, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_0_SIDE_A, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_MGM_2_SIDE_B, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::TEMP_ADC_PAYLOAD_PCDU, new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(tcsCtrl::HEATER_SWITCH_LIST, &heaterSwitchStates);
localDataPoolMap.emplace(tcsCtrl::HEATER_CURRENT, &heaterCurrent);
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(sensorTemperatures.getSid(), false, 10.0));
@ -221,17 +202,21 @@ ReturnValue_t ThermalController::initializeLocalDataPool(localpool::DataPool& lo
subdp::RegularHkPeriodicParams(susTemperatures.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(deviceTemperatures.getSid(), false, 10.0));
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(heaterInfo.getSid(), false, 10.0));
return returnvalue::OK;
}
LocalPoolDataSetBase* ThermalController::getDataSetHandle(sid_t sid) {
switch (sid.ownerSetId) {
case thermalControllerDefinitions::SENSOR_TEMPERATURES:
case tcsCtrl::SENSOR_TEMPERATURES:
return &sensorTemperatures;
case thermalControllerDefinitions::SUS_TEMPERATURES:
case tcsCtrl::SUS_TEMPERATURES:
return &susTemperatures;
case thermalControllerDefinitions::DEVICE_TEMPERATURES:
case tcsCtrl::DEVICE_TEMPERATURES:
return &deviceTemperatures;
case tcsCtrl::HEATER_SET:
return &heaterInfo;
default:
return nullptr;
}
@ -785,7 +770,7 @@ void ThermalController::copyDevices() {
}
{
lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, IMTQ::MCU_TEMPERATURE);
lp_var_t<int16_t> tempMgt = lp_var_t<int16_t>(objects::IMTQ_HANDLER, imtq::MCU_TEMPERATURE);
PoolReadGuard pg(&tempMgt, MutexIF::TimeoutType::WAITING, MUTEX_TIMEOUT);
if (pg.getReadResult() != returnvalue::OK) {
sif::warning << "ThermalController: Failed to read MGT temperature" << std::endl;

22
mission/controller/ThermalController.h
View File

@ -12,6 +12,7 @@
#include <list>
#include "mission/devices/HeaterHandler.h"
#include "mission/devices/devicedefinitions/GomspaceDefinitions.h"
#include "mission/trace.h"
/**
@ -76,9 +77,12 @@ class ThermalController : public ExtendedControllerBase {
HeaterHandler& heaterHandler;
thermalControllerDefinitions::SensorTemperatures sensorTemperatures;
thermalControllerDefinitions::SusTemperatures susTemperatures;
thermalControllerDefinitions::DeviceTemperatures deviceTemperatures;
tcsCtrl::SensorTemperatures sensorTemperatures;
tcsCtrl::SusTemperatures susTemperatures;
tcsCtrl::DeviceTemperatures deviceTemperatures;
tcsCtrl::HeaterInfo heaterInfo;
lp_vec_t<int16_t, 9> currentVecPdu2 =
lp_vec_t<int16_t, 9>(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS));
DeviceHandlerThermalSet imtqThermalSet;
@ -160,11 +164,13 @@ class ThermalController : public ExtendedControllerBase {
std::array<std::pair<bool, double>, 5> sensors;
uint8_t numSensors = 0;
PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075IfBrd = PoolEntry<float>(10.0);
PoolEntry<float> tmp1075Tcs0 = PoolEntry<float>({10.0});
PoolEntry<float> tmp1075Tcs1 = PoolEntry<float>({10.0});
PoolEntry<float> tmp1075PlPcdu0 = PoolEntry<float>({10.0});
PoolEntry<float> tmp1075PlPcdu1 = PoolEntry<float>({10.0});
PoolEntry<float> tmp1075IfBrd = PoolEntry<float>({10.0});
PoolEntry<uint8_t> heaterSwitchStates = PoolEntry<uint8_t>(heater::NUMBER_OF_SWITCHES);
PoolEntry<int16_t> heaterCurrent = PoolEntry<int16_t>();
static constexpr dur_millis_t MUTEX_TIMEOUT = 50;

5
mission/controller/acs/SensorValues.h
View File

@ -1,6 +1,7 @@
#ifndef SENSORVALUES_H_
#define SENSORVALUES_H_
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <mission/devices/devicedefinitions/rwHelpers.h>
#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
@ -10,7 +11,6 @@
#include "mission/devices/devicedefinitions/GPSDefinitions.h"
#include "mission/devices/devicedefinitions/GyroADIS1650XDefinitions.h"
#include "mission/devices/devicedefinitions/SusDefinitions.h"
#include "mission/devices/devicedefinitions/imtqHandlerDefinitions.h"
namespace ACS {
@ -35,7 +35,8 @@ class SensorValues {
MGMLIS3MDL::MgmPrimaryDataset(objects::MGM_2_LIS3_HANDLER);
RM3100::Rm3100PrimaryDataset mgm3Rm3100Set =
RM3100::Rm3100PrimaryDataset(objects::MGM_3_RM3100_HANDLER);
IMTQ::RawMtmMeasurementSet imtqMgmSet = IMTQ::RawMtmMeasurementSet(objects::IMTQ_HANDLER);
imtq::RawMtmMeasurementNoTorque imtqMgmSet =
imtq::RawMtmMeasurementNoTorque(objects::IMTQ_HANDLER);
std::array<SUS::SusDataset, 12> susSets{
SUS::SusDataset(objects::SUS_0_N_LOC_XFYFZM_PT_XF),

33
mission/controller/controllerdefinitions/ThermalControllerDefinitions.h
View File

@ -4,13 +4,16 @@
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
namespace thermalControllerDefinitions {
#include "devices/heaterSwitcherList.h"
enum SetIds : uint32_t {
SENSOR_TEMPERATURES,
DEVICE_TEMPERATURES,
SUS_TEMPERATURES,
COMPONENT_TEMPERATURES
namespace tcsCtrl {
enum SetId : uint32_t {
SENSOR_TEMPERATURES = 0,
DEVICE_TEMPERATURES = 1,
SUS_TEMPERATURES = 2,
COMPONENT_TEMPERATURES = 3,
HEATER_SET = 4,
};
enum PoolIds : lp_id_t {
@ -75,7 +78,10 @@ enum PoolIds : lp_id_t {
TEMP_GYRO_3_SIDE_B,
TEMP_MGM_0_SIDE_A,
TEMP_MGM_2_SIDE_B,
TEMP_ADC_PAYLOAD_PCDU
TEMP_ADC_PAYLOAD_PCDU,
HEATER_SWITCH_LIST,
HEATER_CURRENT
};
static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 25;
@ -202,6 +208,17 @@ class SusTemperatures : public StaticLocalDataSet<ENTRIES_SUS_TEMPERATURE_SET> {
lp_var_t<float>(sid.objectId, PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, this);
};
} // namespace thermalControllerDefinitions
class HeaterInfo : public StaticLocalDataSet<3> {
public:
HeaterInfo(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, HEATER_SET) {}
HeaterInfo(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, HEATER_SET)) {}
lp_vec_t<uint8_t, heater::NUMBER_OF_SWITCHES> heaterSwitchState =
lp_vec_t<uint8_t, heater::NUMBER_OF_SWITCHES>(sid.objectId, PoolIds::HEATER_SWITCH_LIST,
this);
lp_var_t<int16_t> heaterCurrent = lp_var_t<int16_t>(sid.objectId, PoolIds::HEATER_CURRENT, this);
};
} // namespace tcsCtrl
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_THERMALCONTROLLERDEFINITIONS_H_ */

2
mission/core/GenericFactory.cpp
View File

@ -20,6 +20,7 @@
#include <fsfw/pus/Service8FunctionManagement.h>
#include <fsfw/pus/Service9TimeManagement.h>
#include <fsfw/storagemanager/PoolManager.h>
#include <fsfw/subsystem/SubsystemBase.h>
#include <fsfw/tcdistribution/CcsdsDistributor.h>
#include <fsfw/tcdistribution/PusDistributor.h>
#include <fsfw/timemanager/CdsShortTimeStamper.h>
@ -219,6 +220,7 @@ void ObjectFactory::createGenericHeaterComponents(GpioIF& gpioIF, PowerSwitchIF&
}});
heaterHandler = new HeaterHandler(objects::HEATER_HANDLER, &gpioIF, helper, &pwrSwitcher,
pcdu::Switches::PDU2_CH3_TCS_BOARD_HEATER_IN_8V);
heaterHandler->connectModeTreeParent(satsystem::tcs::SUBSYSTEM);
}
void ObjectFactory::createThermalController(HeaterHandler& heaterHandler) {

99
mission/devices/HeaterHandler.cpp
View File

@ -8,11 +8,13 @@
#include <stdexcept>
#include "devices/powerSwitcherList.h"
#include "fsfw/subsystem/helper.h"
HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, HeaterHelper helper,
PowerSwitchIF* mainLineSwitcher_, power::Switch_t mainLineSwitch_)
: SystemObject(setObjectId_),
helper(helper),
modeHelper(this),
gpioInterface(gpioInterface_),
mainLineSwitcher(mainLineSwitcher_),
mainLineSwitch(mainLineSwitch_),
@ -28,8 +30,8 @@ HeaterHandler::HeaterHandler(object_id_t setObjectId_, GpioIF* gpioInterface_, H
if (mainLineSwitcher == nullptr) {
throw std::invalid_argument("HeaterHandler::HeaterHandler: Invalid PowerSwitchIF");
}
heaterMutex = MutexFactory::instance()->createMutex();
if (heaterMutex == nullptr) {
heaterHealthAndStateMutex = MutexFactory::instance()->createMutex();
if (heaterHealthAndStateMutex == nullptr) {
throw std::runtime_error("HeaterHandler::HeaterHandler: Creating Mutex failed");
}
auto mqArgs = MqArgs(setObjectId_, static_cast<void*>(this));
@ -46,6 +48,13 @@ ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
heater.first->performOperation(0);
}
handleSwitchHandling();
if (waitForSwitchOff) {
if (mainLineSwitcher->getSwitchState(mainLineSwitch) == SWITCH_OFF) {
waitForSwitchOff = false;
mode = MODE_OFF;
modeHelper.modeChanged(mode, submode);
}
}
} catch (const std::out_of_range& e) {
sif::warning << "HeaterHandler::performOperation: "
"Out of range error | "
@ -76,6 +85,10 @@ ReturnValue_t HeaterHandler::initialize() {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
result = modeHelper.initialize();
if (result != returnvalue::OK) {
return ObjectManagerIF::CHILD_INIT_FAILED;
}
return returnvalue::OK;
}
@ -101,6 +114,10 @@ void HeaterHandler::readCommandQueue() {
if (result == returnvalue::OK) {
continue;
}
result = modeHelper.handleModeCommand(&command);
if (result == returnvalue::OK) {
continue;
}
} while (result == returnvalue::OK);
}
@ -125,7 +142,11 @@ ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId, MessageQueueId_t
auto action = static_cast<SwitchAction>(data[1]);
// Always accepts OFF commands
if (action == SwitchAction::SET_SWITCH_ON) {
HasHealthIF::HealthState health = heater.healthDevice->getHealth();
HasHealthIF::HealthState health;
{
MutexGuard mg(heaterHealthAndStateMutex);
health = heater.healthDevice->getHealth();
}
if (health == HasHealthIF::FAULTY or health == HasHealthIF::PERMANENT_FAULTY or
health == HasHealthIF::NEEDS_RECOVERY) {
return HasHealthIF::OBJECT_NOT_HEALTHY;
@ -219,6 +240,9 @@ void HeaterHandler::handleSwitchHandling() {
void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
ReturnValue_t result = returnvalue::OK;
auto& heater = heaterVec.at(heaterIdx);
if (waitForSwitchOff) {
waitForSwitchOff = false;
}
/* Check if command waits for main switch being set on and whether the timeout has expired */
if (heater.waitMainSwitchOn && heater.mainSwitchCountdown.hasTimedOut()) {
// TODO - This requires the initiation of an FDIR procedure
@ -245,11 +269,16 @@ void HeaterHandler::handleSwitchOnCommand(heater::Switchers heaterIdx) {
triggerEvent(GPIO_PULL_HIGH_FAILED, result);
} else {
triggerEvent(HEATER_WENT_ON, heaterIdx, 0);
heater.switchState = ON;
{
MutexGuard mg(heaterHealthAndStateMutex);
heater.switchState = ON;
}
}
} else {
triggerEvent(SWITCH_ALREADY_ON, heaterIdx);
}
mode = HasModesIF::MODE_ON;
modeHelper.modeChanged(mode, submode);
// There is no need to send action finish replies if the sender was the
// HeaterHandler itself
if (heater.replyQueue != commandQueue->getId()) {
@ -290,15 +319,15 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
<< " low" << std::endl;
triggerEvent(GPIO_PULL_LOW_FAILED, result);
} else {
result = heaterMutex->lockMutex();
heater.switchState = OFF;
if (result == returnvalue::OK) {
heaterMutex->unlockMutex();
{
MutexGuard mg(heaterHealthAndStateMutex);
heater.switchState = OFF;
}
triggerEvent(HEATER_WENT_OFF, heaterIdx, 0);
// When all switches are off, also main line switch will be turned off
if (allSwitchesOff()) {
mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
waitForSwitchOff = true;
}
}
} else {
@ -317,7 +346,7 @@ void HeaterHandler::handleSwitchOffCommand(heater::Switchers heaterIdx) {
}
HeaterHandler::SwitchState HeaterHandler::checkSwitchState(heater::Switchers switchNr) const {
MutexGuard mg(heaterMutex);
MutexGuard mg(heaterHealthAndStateMutex);
return heaterVec.at(switchNr).switchState;
}
@ -330,9 +359,57 @@ ReturnValue_t HeaterHandler::switchHeater(heater::Switchers heater, SwitchState
return returnvalue::FAILED;
}
void HeaterHandler::announceMode(bool recursive) {
triggerEvent(MODE_INFO, mode, submode);
std::array<SwitchState, 8> states;
getAllSwitchStates(states);
for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
if (states[idx] == ON) {
EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_ON, 0);
} else {
EventManagerIF::triggerEvent(helper.heaters[idx].first->getObjectId(), MODE_INFO, MODE_OFF,
0);
}
}
}
void HeaterHandler::getMode(Mode_t* mode, Submode_t* submode) {
if (!mode || !submode) {
return;
}
*mode = this->mode;
*submode = this->submode;
}
const HasHealthIF* HeaterHandler::getOptHealthIF() const { return nullptr; }
const HasModesIF& HeaterHandler::getModeIF() const { return *this; }
ReturnValue_t HeaterHandler::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
}
ModeTreeChildIF& HeaterHandler::getModeTreeChildIF() { return *this; }
object_id_t HeaterHandler::getObjectId() const { return SystemObject::getObjectId(); }
ReturnValue_t HeaterHandler::getAllSwitchStates(std::array<SwitchState, 8>& statesBuf) {
{
MutexGuard mg(heaterHealthAndStateMutex);
if (mg.getLockResult() != returnvalue::OK) {
return returnvalue::FAILED;
}
for (unsigned idx = 0; idx < helper.heaters.size(); idx++) {
statesBuf[idx] = heaterVec[idx].switchState;
}
}
return returnvalue::OK;
}
bool HeaterHandler::allSwitchesOff() {
bool allSwitchesOrd = false;
MutexGuard mg(heaterMutex);
MutexGuard mg(heaterHealthAndStateMutex);
/* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
for (power::Switch_t switchNr = 0; switchNr < heater::NUMBER_OF_SWITCHES; switchNr++) {
allSwitchesOrd = allSwitchesOrd || heaterVec.at(switchNr).switchState;
@ -365,7 +442,7 @@ uint32_t HeaterHandler::getSwitchDelayMs(void) const { return 2000; }
HasHealthIF::HealthState HeaterHandler::getHealth(heater::Switchers heater) {
auto* healthDev = heaterVec.at(heater).healthDevice;
if (healthDev != nullptr) {
MutexGuard mg(heaterMutex);
MutexGuard mg(heaterHealthAndStateMutex);
return healthDev->getHealth();
}
return HasHealthIF::HealthState::FAULTY;

27
mission/devices/HeaterHandler.h
View File

@ -10,6 +10,8 @@
#include <fsfw/objectmanager/SystemObject.h>
#include <fsfw/power/PowerSwitchIF.h>
#include <fsfw/returnvalues/returnvalue.h>
#include <fsfw/subsystem/ModeTreeChildIF.h>
#include <fsfw/subsystem/ModeTreeConnectionIF.h>
#include <fsfw/tasks/ExecutableObjectIF.h>
#include <fsfw/timemanager/Countdown.h>
#include <fsfw_hal/common/gpio/GpioIF.h>
@ -40,6 +42,9 @@ struct HeaterHelper {
*/
class HeaterHandler : public ExecutableObjectIF,
public PowerSwitchIF,
public HasModesIF,
public ModeTreeChildIF,
public ModeTreeConnectionIF,
public SystemObject,
public HasActionsIF {
friend class ThermalController;
@ -54,6 +59,7 @@ class HeaterHandler : public ExecutableObjectIF,
static const ReturnValue_t COMMAND_ALREADY_WAITING = MAKE_RETURN_CODE(0xA5);
enum CmdSourceParam : uint8_t { INTERNAL = 0, EXTERNAL = 1 };
enum SwitchState : uint8_t { ON = 1, OFF = 0 };
/** Device command IDs */
static const DeviceCommandId_t SWITCH_HEATER = 0x0;
@ -61,10 +67,12 @@ class HeaterHandler : public ExecutableObjectIF,
HeaterHandler(object_id_t setObjectId, GpioIF* gpioInterface_, HeaterHelper helper,
PowerSwitchIF* mainLineSwitcherObjectId, power::Switch_t mainLineSwitch);
ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
ReturnValue_t getAllSwitchStates(std::array<SwitchState, 8>& statesBuf);
virtual ~HeaterHandler();
protected:
enum SwitchState : bool { ON = true, OFF = false };
enum SwitchAction : uint8_t { SET_SWITCH_OFF, SET_SWITCH_ON, NONE };
ReturnValue_t switchHeater(heater::Switchers heater, SwitchState switchState);
@ -128,12 +136,14 @@ class HeaterHandler : public ExecutableObjectIF,
HeaterMap heaterVec = {};
MutexIF* heaterMutex = nullptr;
MutexIF* heaterHealthAndStateMutex = nullptr;
HeaterHelper helper;
ModeHelper modeHelper;
/** Size of command queue */
size_t cmdQueueSize = 20;
bool waitForSwitchOff = true;
GpioIF* gpioInterface = nullptr;
@ -152,6 +162,9 @@ class HeaterHandler : public ExecutableObjectIF,
StorageManagerIF* ipcStore = nullptr;
Mode_t mode = HasModesIF::MODE_OFF;
Submode_t submode = 0;
void readCommandQueue();
/**
@ -172,6 +185,16 @@ class HeaterHandler : public ExecutableObjectIF,
*/
void setInitialSwitchStates();
// HasModesIF implementation
void announceMode(bool recursive) override;
void getMode(Mode_t* mode, Submode_t* submode) override;
// Mode Tree helper overrides
object_id_t getObjectId() const override;
const HasHealthIF* getOptHealthIF() const override;
const HasModesIF& getModeIF() const override;
ModeTreeChildIF& getModeTreeChildIF() override;
void handleSwitchOnCommand(heater::Switchers heaterIdx);
void handleSwitchOffCommand(heater::Switchers heaterIdx);

1267
mission/devices/ImtqHandler.cpp
View File

File diff suppressed because it is too large Load Diff

94
mission/devices/ImtqHandler.h
View File

@ -2,7 +2,7 @@
#define MISSION_DEVICES_IMTQHANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <mission/devices/devicedefinitions/imtqHandlerDefinitions.h>
#include <mission/devices/devicedefinitions/imtqHelpers.h>
#include <string.h>
#include "events/subsystemIdRanges.h"
@ -23,7 +23,6 @@ class ImtqHandler : public DeviceHandlerBase {
void setPollingMode(NormalPollingMode pollMode);
void doSendRead() override;
/**
* @brief Sets mode to MODE_NORMAL. Can be used for debugging.
*/
@ -32,6 +31,7 @@ class ImtqHandler : public DeviceHandlerBase {
void setDebugMode(bool enable);
protected:
ReturnValue_t performOperation(uint8_t opCode);
void doStartUp() override;
void doShutDown() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override;
@ -42,7 +42,6 @@ class ImtqHandler : public DeviceHandlerBase {
ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId,
size_t* foundLen) override;
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override;
void setNormalDatapoolEntriesInvalid() override;
virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@ -50,19 +49,6 @@ class ImtqHandler : public DeviceHandlerBase {
ReturnValue_t getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) override;
private:
static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
static const ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0xA0);
static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(0xA1);
static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(0xA2);
static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(0xA3);
static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(0xA4);
static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(0xA5);
static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(0xA6);
//! [EXPORT] : [COMMENT] The status reply to a self test command was received but no self test
//! command has been sent. This should normally never happen.
static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::IMTQ_HANDLER;
//! [EXPORT] : [COMMENT] Get self test result returns I2C failure
@ -97,49 +83,43 @@ class ImtqHandler : public DeviceHandlerBase {
//! link between IMTQ and OBC.
static const Event INVALID_ERROR_BYTE = MAKE_EVENT(8, severity::LOW);
IMTQ::EngHkDataset engHkDataset;
IMTQ::CalibratedMtmMeasurementSet calMtmMeasurementSet;
IMTQ::RawMtmMeasurementSet rawMtmMeasurementSet;
IMTQ::DipoleActuationSet dipoleSet;
IMTQ::PosXSelfTestSet posXselfTestDataset;
IMTQ::NegXSelfTestSet negXselfTestDataset;
IMTQ::PosYSelfTestSet posYselfTestDataset;
IMTQ::NegYSelfTestSet negYselfTestDataset;
IMTQ::PosZSelfTestSet posZselfTestDataset;
IMTQ::NegZSelfTestSet negZselfTestDataset;
imtq::StatusDataset statusSet;
imtq::DipoleActuationSet dipoleSet;
imtq::RawMtmMeasurementNoTorque rawMtmNoTorque;
imtq::HkDatasetNoTorque hkDatasetNoTorque;
imtq::RawMtmMeasurementWithTorque rawMtmWithTorque;
imtq::HkDatasetWithTorque hkDatasetWithTorque;
imtq::CalibratedMtmMeasurementSet calMtmMeasurementSet;
imtq::PosXSelfTestSet posXselfTestDataset;
imtq::NegXSelfTestSet negXselfTestDataset;
imtq::PosYSelfTestSet posYselfTestDataset;
imtq::NegYSelfTestSet negYselfTestDataset;
imtq::PosZSelfTestSet posZselfTestDataset;
imtq::NegZSelfTestSet negZselfTestDataset;
NormalPollingMode pollingMode = NormalPollingMode::UNCALIBRATED;
PoolEntry<uint8_t> statusMode = PoolEntry<uint8_t>({0});
PoolEntry<uint8_t> statusError = PoolEntry<uint8_t>({0});
PoolEntry<uint8_t> statusConfig = PoolEntry<uint8_t>({0});
PoolEntry<uint32_t> statusUptime = PoolEntry<uint32_t>({0});
PoolEntry<int32_t> mgmCalEntry = PoolEntry<int32_t>(3);
PoolEntry<int16_t> dipoleXEntry = PoolEntry<int16_t>(0, false);
PoolEntry<int16_t> dipoleYEntry = PoolEntry<int16_t>(0, false);
PoolEntry<int16_t> dipoleZEntry = PoolEntry<int16_t>(0, false);
PoolEntry<uint16_t> torqueDurationEntry = PoolEntry<uint16_t>(0, false);
// Hardcoded to default integration time of 10 ms.
// SHOULDDO: Support for other integration times
Countdown integrationTimeCd = Countdown(10);
power::Switch_t switcher = power::NO_SWITCH;
uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE];
uint8_t commandBuffer[imtq::MAX_COMMAND_SIZE];
bool goToNormalMode = false;
bool debugMode = false;
bool specialRequestActive = false;
bool firstReplyCycle = true;
enum class CommunicationStep {
GET_ENG_HK_DATA,
START_MTM_MEASUREMENT,
GET_CAL_MTM_MEASUREMENT,
GET_RAW_MTM_MEASUREMENT,
DIPOLE_ACTUATION
};
CommunicationStep communicationStep = CommunicationStep::GET_ENG_HK_DATA;
enum class StartupStep { NONE, COMMAND_SELF_TEST, GET_SELF_TEST_RESULT };
StartupStep startupStep = StartupStep::COMMAND_SELF_TEST;
bool selfTestPerformed = false;
imtq::RequestType requestStep = imtq::RequestType::MEASURE_NO_ACTUATION;
/**
* @brief In case of a status reply to a single axis self test command, this function
@ -155,7 +135,7 @@ class ImtqHandler : public DeviceHandlerBase {
*
* @return The return code derived from the received status byte.
*/
ReturnValue_t parseStatusByte(const uint8_t* packet);
ReturnValue_t parseStatusByte(imtq::CC::CC command, const uint8_t* packet);
/**
* @brief This function fills the engineering housekeeping dataset with the received data.
@ -163,23 +143,9 @@ class ImtqHandler : public DeviceHandlerBase {
* @param packet Pointer to the received data.
*
*/
void fillEngHkDataset(const uint8_t* packet);
void fillEngHkDataset(imtq::HkDataset& hkDataset, const uint8_t* packet);
/**
* @brief This function sends a command reply to the requesting queue.
*
* @param data Pointer to the data to send.
* @param dataSize Size of the data to send.
* @param relplyId Reply id which will be inserted at the beginning of the action message.
*/
void handleDeviceTM(const uint8_t* data, size_t dataSize, DeviceCommandId_t replyId);
/**
* @brief This function handles the reply containing the commanded dipole.
*
* @param packet Pointer to the reply data.
*/
void handleGetCommandedDipoleReply(const uint8_t* packet);
void fillSystemStateIntoDataset(const uint8_t* packet);
/**
* @brief This function parses the reply containing the calibrated MTM measurement and writes
@ -212,7 +178,7 @@ class ImtqHandler : public DeviceHandlerBase {
void handlePositiveZSelfTestReply(const uint8_t* packet);
void handleNegativeZSelfTestReply(const uint8_t* packet);
ReturnValue_t buildDipoleActuationCommand();
// ReturnValue_t buildDipoleActuationCommand();
/**
* @brief This function checks the error byte of a self test measurement.
*

3
mission/devices/devicedefinitions/CMakeLists.txt
View File

@ -1 +1,2 @@
target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp)
target_sources(${LIB_EIVE_MISSION} PRIVATE ScexDefinitions.cpp rwHelpers.cpp
imtqHelpers.cpp)

49
mission/devices/devicedefinitions/imtqHelpers.cpp Normal file
View File

@ -0,0 +1,49 @@
#include "imtqHelpers.h"
size_t imtq::getReplySize(CC::CC cc, size_t* optSecondSize) {
switch (cc) {
// Software reset is a bit special and can also cause a I2C NAK because
// the device might be reset at that moment. Otherwise, 2 bytes should be returned
case (CC::CC::SOFTWARE_RESET): {
if (optSecondSize != nullptr) {
*optSecondSize = 0;
}
return 2;
}
case (CC::CC::START_ACTUATION_DIPOLE):
case (CC::CC::SELF_TEST_CMD):
case (CC::CC::START_MTM_MEASUREMENT): {
return 2;
}
case (CC::CC::GET_SYSTEM_STATE): {
return 9;
}
case (CC::CC::GET_RAW_MTM_MEASUREMENT):
case (CC::CC::GET_CAL_MTM_MEASUREMENT): {
return 15;
}
case (CC::CC::GET_COIL_CURRENT):
case (CC::CC::GET_COMMANDED_DIPOLE):
case (CC::CC::GET_COIL_TEMPERATURES): {
return 8;
}
case (CC::CC::GET_SELF_TEST_RESULT): {
// Can also be 360 for the all axes self-test!
if (optSecondSize != nullptr) {
*optSecondSize = 360;
}
return 120;
}
case (CC::CC::GET_RAW_HK_DATA):
case (CC::CC::GET_ENG_HK_DATA): {
return 24;
}
case (CC::CC::GET_PARAM): {
return imtq::replySize::MAX_SET_GET_PARAM_LEN;
}
default: {
return 0;
}
}
return 0;
}

438
mission/devices/devicedefinitions/imtqHandlerDefinitions.h → mission/devices/devicedefinitions/imtqHelpers.h
View File

@ -1,23 +1,60 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_
#include <eive/resultClassIds.h>
#include <fsfw/datapool/PoolReadGuard.h>
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
class ImtqHandler;
namespace IMTQ {
namespace imtq {
static const DeviceCommandId_t NONE = 0x0;
static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1;
enum ComStep : uint8_t {
DHB_OP = 0,
START_MEASURE_SEND = 1,
START_MEASURE_GET = 2,
READ_MEASURE_SEND = 3,
READ_MEASURE_GET = 4,
START_ACTUATE_SEND = 5,
START_ACTUATE_GET = 6,
READ_ACTUATE_SEND = 7,
READ_ACTUATE_GET = 8,
};
enum class RequestType : uint8_t { MEASURE_NO_ACTUATION, ACTUATE };
enum class SpecialRequest : uint8_t {
NONE = 0,
DO_SELF_TEST_POS_X = 1,
DO_SELF_TEST_NEG_X = 2,
DO_SELF_TEST_POS_Y = 3,
DO_SELF_TEST_NEG_Y = 4,
DO_SELF_TEST_POS_Z = 5,
DO_SELF_TEST_NEG_Z = 6,
GET_SELF_TEST_RESULT = 7
};
static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER;
static constexpr ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0);
static constexpr ReturnValue_t MGM_MEASUREMENT_LOW_LEVEL_ERROR = MAKE_RETURN_CODE(1);
static constexpr ReturnValue_t ACTUATE_CMD_LOW_LEVEL_ERROR = MAKE_RETURN_CODE(2);
static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(3);
static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(4);
static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(5);
static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(6);
static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(7);
static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(8);
//! [EXPORT] : [COMMENT] The status reply to a self test command was received but no self test
//! command has been sent. This should normally never happen.
static const ReturnValue_t UNEXPECTED_SELF_TEST_REPLY = MAKE_RETURN_CODE(0xA7);
namespace cmdIds {
static constexpr DeviceCommandId_t REQUEST = 0x70;
static constexpr DeviceCommandId_t REPLY = 0x71;
static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2;
static const DeviceCommandId_t GET_COMMANDED_DIPOLE = 0x3;
/** Generates new measurement of the magnetic field */
static const DeviceCommandId_t START_MTM_MEASUREMENT = 0x4;
/** Requests the calibrated magnetometer measurement */
static const DeviceCommandId_t GET_CAL_MTM_MEASUREMENT = 0x5;
/** Requests the raw values measured by the built-in MTM XEN1210 */
static const DeviceCommandId_t GET_RAW_MTM_MEASUREMENT = 0x6;
static const DeviceCommandId_t POS_X_SELF_TEST = 0x7;
static const DeviceCommandId_t NEG_X_SELF_TEST = 0x8;
static const DeviceCommandId_t POS_Y_SELF_TEST = 0x9;
@ -26,36 +63,49 @@ static const DeviceCommandId_t POS_Z_SELF_TEST = 0xB;
static const DeviceCommandId_t NEG_Z_SELF_TEST = 0xC;
static const DeviceCommandId_t GET_SELF_TEST_RESULT = 0xD;
static const uint8_t GET_TEMP_REPLY_SIZE = 2;
static const uint8_t CFGR_CMD_SIZE = 3;
} // namespace cmdIds
static const uint8_t POINTER_REG_SIZE = 1;
enum SetIds : uint32_t {
ENG_HK = 1,
CAL_MGM = 2,
RAW_MGM = 3,
POS_X_TEST = 4,
NEG_X_TEST = 5,
POS_Y_TEST = 6,
NEG_Y_TEST = 7,
POS_Z_TEST = 8,
NEG_Z_TEST = 9,
DIPOLES = 10
ENG_HK_NO_TORQUE = 1,
RAW_MTM_NO_TORQUE = 2,
ENG_HK_SET_WITH_TORQUE = 3,
RAW_MTM_WITH_TORQUE = 4,
STATUS_SET = 5,
DIPOLES = 6,
CAL_MTM_SET = 9,
POSITIVE_X_TEST = 10,
NEGATIVE_X_TEST = 11,
POSITIVE_Y_TEST = 12,
NEGATIVE_Y_TEST = 13,
POSITIVE_Z_TEST = 14,
NEGATIVE_Z_TEST = 15,
};
static const uint8_t SIZE_ENG_HK_COMMAND = 1;
static const uint8_t SIZE_STATUS_REPLY = 2;
static const uint8_t SIZE_ENG_HK_DATA_REPLY = 24;
static const uint8_t SIZE_GET_COMMANDED_DIPOLE_REPLY = 8;
static const uint8_t SIZE_GET_CAL_MTM_MEASUREMENT = 15;
static const uint8_t SIZE_GET_RAW_MTM_MEASUREMENT = 15;
static const uint16_t SIZE_SELF_TEST_RESULTS = 120;
namespace replySize {
static const uint16_t MAX_REPLY_SIZE = SIZE_SELF_TEST_RESULTS;
static const uint8_t MAX_COMMAND_SIZE = 9;
static constexpr uint8_t GET_TEMP_REPLY_SIZE = 2;
static constexpr uint8_t CFGR_CMD_SIZE = 3;
static constexpr size_t DEFAULT_MIN_LEN = 2;
static constexpr size_t STATUS_REPLY = DEFAULT_MIN_LEN;
static constexpr size_t ENG_HK_DATA_REPLY = 24;
static constexpr size_t GET_COMMANDED_DIPOLE_REPLY = 8;
static constexpr size_t MAX_SET_GET_PARAM_LEN = 12;
static constexpr size_t SYSTEM_STATE = 9;
static constexpr size_t CAL_MTM_MEASUREMENT = 15;
static constexpr size_t RAW_MTM_MEASUREMENT = 15;
static constexpr size_t SELF_TEST_RESULTS = 120;
static constexpr size_t SELF_TEST_RESULTS_ALL_AXES = 360;
} // namespace replySize
static const uint16_t MAX_REPLY_SIZE = replySize::SELF_TEST_RESULTS_ALL_AXES;
static const uint8_t MAX_COMMAND_SIZE = 16;
/** Define entries in IMTQ specific dataset */
static const uint8_t ENG_HK_SET_POOL_ENTRIES = 11;
static const uint8_t HK_SET_POOL_ENTRIES = 20;
static const uint8_t CAL_MTM_POOL_ENTRIES = 4;
static const uint8_t SELF_TEST_DATASET_ENTRIES = 104;
@ -72,45 +122,75 @@ static const uint8_t INVALID_ERROR_BYTE =
static const uint8_t MAIN_STEP_OFFSET = 43;
// Command Code
namespace CC {
/**
* Command code definitions. Each command or reply of an IMTQ request will begin with one of
* the following command codes.
*/
namespace CC {
static const uint8_t START_MTM_MEASUREMENT = 0x4;
static const uint8_t START_ACTUATION_DIPOLE = 0x6;
static const uint8_t SELF_TEST_CMD = 0x8;
static const uint8_t SOFTWARE_RESET = 0xAA;
static const uint8_t GET_ENG_HK_DATA = 0x4A;
static const uint8_t GET_COMMANDED_DIPOLE = 0x46;
static const uint8_t GET_RAW_MTM_MEASUREMENT = 0x42;
static const uint8_t GET_CAL_MTM_MEASUREMENT = 0x43;
static const uint8_t GET_SELF_TEST_RESULT = 0x47;
static const uint8_t PAST_AVAILABLE_RESPONSE_BYTES = 0xff;
}; // namespace CC
enum CC : uint8_t {
START_MTM_MEASUREMENT = 0x4,
START_ACTUATION_DIPOLE = 0x6,
SELF_TEST_CMD = 0x8,
GET_SYSTEM_STATE = 0x41,
GET_RAW_MTM_MEASUREMENT = 0x42,
GET_CAL_MTM_MEASUREMENT = 0x43,
GET_COIL_CURRENT = 0x44,
GET_COIL_TEMPERATURES = 0x45,
GET_COMMANDED_DIPOLE = 0x46,
GET_SELF_TEST_RESULT = 0x47,
GET_RAW_HK_DATA = 0x49,
GET_ENG_HK_DATA = 0x4A,
GET_PARAM = 0x81,
SET_PARAM = 0x82,
SOFTWARE_RESET = 0xAA,
PAST_AVAILABLE_RESPONSE_BYTES = 0xff
};
namespace SELF_TEST_AXIS {
static const uint8_t ALL = 0x0;
static const uint8_t X_POSITIVE = 0x1;
static const uint8_t X_NEGATIVE = 0x2;
static const uint8_t Y_POSITIVE = 0x3;
static const uint8_t Y_NEGATIVE = 0x4;
static const uint8_t Z_POSITIVE = 0x5;
static const uint8_t Z_NEGATIVE = 0x6;
} // namespace SELF_TEST_AXIS
} // namespace CC
namespace SELF_TEST_STEPS {
static const uint8_t INIT = 0x0;
static const uint8_t X_POSITIVE = 0x1;
static const uint8_t X_NEGATIVE = 0x2;
static const uint8_t Y_POSITIVE = 0x3;
static const uint8_t Y_NEGATIVE = 0x4;
static const uint8_t Z_POSITIVE = 0x5;
static const uint8_t Z_NEGATIVE = 0x6;
static const uint8_t FINA = 0x7;
} // namespace SELF_TEST_STEPS
size_t getReplySize(CC::CC cc, size_t* optSecondSize = nullptr);
namespace mode {
enum Mode : uint8_t { IDLE = 0, SELF_TEST = 1, DETUMBLE = 2 };
}
namespace selfTest {
enum Axis : uint8_t {
ALL = 0x0,
X_POSITIVE = 0x1,
X_NEGATIVE = 0x2,
Y_POSITIVE = 0x3,
Y_NEGATIVE = 0x4,
Z_POSITIVE = 0x5,
Z_NEGATIVE = 0x6,
};
namespace step {
enum Step : uint8_t {
INIT = 0x0,
X_POSITIVE = 0x1,
X_NEGATIVE = 0x2,
Y_POSITIVE = 0x3,
Y_NEGATIVE = 0x4,
Z_POSITIVE = 0x5,
Z_NEGATIVE = 0x6,
FINA = 0x7
};
}
} // namespace selfTest
enum PoolIds : lp_id_t {
STATUS_BYTE_MODE,
STATUS_BYTE_ERROR,
STATUS_BYTE_CONF,
STATUS_BYTE_UPTIME,
DIGITAL_VOLTAGE_MV,
ANALOG_VOLTAGE_MV,
DIGITAL_CURRENT,
@ -384,12 +464,23 @@ enum PoolIds : lp_id_t {
FINA_NEG_Z_COIL_Z_TEMPERATURE,
};
class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
class StatusDataset : public StaticLocalDataSet<4> {
public:
EngHkDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, IMTQ::SetIds::ENG_HK) {}
StatusDataset(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, imtq::SetIds::STATUS_SET) {}
// Status byte variables
lp_var_t<uint8_t> statusByteMode = lp_var_t<uint8_t>(sid.objectId, STATUS_BYTE_MODE, this);
lp_var_t<uint8_t> statusByteError = lp_var_t<uint8_t>(sid.objectId, STATUS_BYTE_ERROR, this);
lp_var_t<uint8_t> statusByteConfig = lp_var_t<uint8_t>(sid.objectId, STATUS_BYTE_CONF, this);
lp_var_t<uint32_t> statusByteUptime = lp_var_t<uint32_t>(sid.objectId, STATUS_BYTE_UPTIME, this);
};
EngHkDataset(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::ENG_HK)) {}
class HkDataset : public StaticLocalDataSet<HK_SET_POOL_ENTRIES> {
public:
HkDataset(HasLocalDataPoolIF* owner, uint32_t setId) : StaticLocalDataSet(owner, setId) {}
HkDataset(object_id_t objectId, uint32_t setId) : StaticLocalDataSet(sid_t(objectId, setId)) {}
// Engineering HK variables
lp_var_t<uint16_t> digitalVoltageMv = lp_var_t<uint16_t>(sid.objectId, DIGITAL_VOLTAGE_MV, this);
lp_var_t<uint16_t> analogVoltageMv = lp_var_t<uint16_t>(sid.objectId, ANALOG_VOLTAGE_MV, this);
lp_var_t<float> digitalCurrentmA = lp_var_t<float>(sid.objectId, DIGITAL_CURRENT, this);
@ -404,16 +495,27 @@ class EngHkDataset : public StaticLocalDataSet<ENG_HK_SET_POOL_ENTRIES> {
lp_var_t<int16_t> mcuTemperature = lp_var_t<int16_t>(sid.objectId, MCU_TEMPERATURE, this);
};
class HkDatasetNoTorque : public HkDataset {
public:
HkDatasetNoTorque(HasLocalDataPoolIF* owner) : HkDataset(owner, imtq::SetIds::ENG_HK_NO_TORQUE) {}
};
class HkDatasetWithTorque : public HkDataset {
public:
HkDatasetWithTorque(HasLocalDataPoolIF* owner)
: HkDataset(owner, imtq::SetIds::ENG_HK_SET_WITH_TORQUE) {}
};
/**
*
* @brief This dataset holds the last calibrated MTM measurement.
*/
class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
public:
CalibratedMtmMeasurementSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::CAL_MGM) {}
: StaticLocalDataSet(owner, imtq::SetIds::CAL_MTM_SET) {}
CalibratedMtmMeasurementSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::CAL_MGM)) {}
: StaticLocalDataSet(sid_t(objectId, imtq::SetIds::CAL_MTM_SET)) {}
/** The unit of all measurements is nT */
lp_vec_t<int32_t, 3> mgmXyz = lp_vec_t<int32_t, 3>(sid.objectId, MGM_CAL_NT, this);
@ -427,11 +529,11 @@ class CalibratedMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRI
*/
class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
public:
RawMtmMeasurementSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::RAW_MGM) {}
RawMtmMeasurementSet(HasLocalDataPoolIF* owner, uint32_t setId)
: StaticLocalDataSet(owner, setId) {}
RawMtmMeasurementSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::RAW_MGM)) {}
RawMtmMeasurementSet(object_id_t objectId, uint32_t setId)
: StaticLocalDataSet(sid_t(objectId, setId)) {}
/** The unit of all measurements is nT */
lp_vec_t<float, 3> mtmRawNt = lp_vec_t<float, 3>(sid.objectId, MTM_RAW, this);
@ -440,6 +542,21 @@ class RawMtmMeasurementSet : public StaticLocalDataSet<CAL_MTM_POOL_ENTRIES> {
lp_var_t<uint8_t>(sid.objectId, ACTUATION_RAW_STATUS, this);
};
class RawMtmMeasurementNoTorque : public RawMtmMeasurementSet {
public:
RawMtmMeasurementNoTorque(HasLocalDataPoolIF* owner)
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
RawMtmMeasurementNoTorque(object_id_t objectId)
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_NO_TORQUE) {}
};
class RawMtmMeasurementWithTorque : public RawMtmMeasurementSet {
public:
RawMtmMeasurementWithTorque(HasLocalDataPoolIF* owner)
: RawMtmMeasurementSet(owner, imtq::SetIds::RAW_MTM_WITH_TORQUE) {}
RawMtmMeasurementWithTorque(object_id_t objectId)
: RawMtmMeasurementSet(objectId, imtq::SetIds::RAW_MTM_WITH_TORQUE) {}
};
/**
* @brief This class can be used to ease the generation of an action message commanding the
* IMTQHandler to configure the magnettorquer with the desired dipoles.
@ -482,9 +599,9 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
public:
DipoleActuationSet(HasLocalDataPoolIF& owner)
: StaticLocalDataSet(&owner, IMTQ::SetIds::DIPOLES) {}
: StaticLocalDataSet(&owner, imtq::SetIds::DIPOLES) {}
DipoleActuationSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {}
: StaticLocalDataSet(sid_t(objectId, imtq::SetIds::DIPOLES)) {}
// Refresh torque command without changing any of the set dipoles.
void refreshTorqueing(uint16_t durationMs_) { currentTorqueDurationMs = durationMs_; }
@ -534,10 +651,10 @@ class DipoleActuationSet : public StaticLocalDataSet<4> {
class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosXSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_X_TEST) {}
: StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_X_TEST) {}
PosXSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_X_TEST)) {}
: StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_X_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_X_ERR, this);
@ -611,10 +728,10 @@ class PosXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegXSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_X_TEST) {}
: StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_X_TEST) {}
NegXSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_X_TEST)) {}
: StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_X_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_X_ERR, this);
@ -688,10 +805,10 @@ class NegXSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosYSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_Y_TEST) {}
: StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_Y_TEST) {}
PosYSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Y_TEST)) {}
: StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_Y_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Y_ERR, this);
@ -765,10 +882,10 @@ class PosYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegYSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Y_TEST) {}
: StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_Y_TEST) {}
NegYSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Y_TEST)) {}
: StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_Y_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Y_ERR, this);
@ -842,10 +959,10 @@ class NegYSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
PosZSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::POS_Z_TEST) {}
: StaticLocalDataSet(owner, imtq::SetIds::POSITIVE_Z_TEST) {}
PosZSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::POS_Z_TEST)) {}
: StaticLocalDataSet(sid_t(objectId, imtq::SetIds::POSITIVE_Y_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_POS_Z_ERR, this);
@ -919,10 +1036,10 @@ class PosZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
public:
NegZSelfTestSet(HasLocalDataPoolIF* owner)
: StaticLocalDataSet(owner, IMTQ::SetIds::NEG_Z_TEST) {}
: StaticLocalDataSet(owner, imtq::SetIds::NEGATIVE_Z_TEST) {}
NegZSelfTestSet(object_id_t objectId)
: StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::NEG_Z_TEST)) {}
: StaticLocalDataSet(sid_t(objectId, imtq::SetIds::NEGATIVE_Z_TEST)) {}
/** INIT block */
lp_var_t<uint8_t> initErr = lp_var_t<uint8_t>(sid.objectId, INIT_NEG_Z_ERR, this);
@ -979,6 +1096,153 @@ class NegZSelfTestSet : public StaticLocalDataSet<SELF_TEST_DATASET_ENTRIES> {
lp_var_t<int16_t>(sid.objectId, FINA_NEG_Z_COIL_Z_TEMPERATURE, this);
};
} // namespace IMTQ
} // namespace imtq
struct ImtqRequest {
friend class ImtqHandler;
public:
static constexpr size_t REQUEST_LEN = 10;
ImtqRequest(const uint8_t* data, size_t maxSize)
: rawData(const_cast<uint8_t*>(data)), maxSize(maxSize) {}
imtq::RequestType getRequestType() const { return static_cast<imtq::RequestType>(rawData[0]); }
void setMeasureRequest(imtq::SpecialRequest specialRequest) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_NO_ACTUATION);
rawData[1] = static_cast<uint8_t>(specialRequest);
}
void setActuateRequest(int16_t dipoleX, int16_t dipoleY, int16_t dipoleZ,
uint16_t torqueDuration) {
size_t dummy = 0;
rawData[0] = static_cast<uint8_t>(imtq::RequestType::ACTUATE);
rawData[1] = static_cast<uint8_t>(imtq::SpecialRequest::NONE);
uint8_t* serPtr = rawData + 2;
SerializeAdapter::serialize(&dipoleX, &serPtr, &dummy, maxSize,
SerializeIF::Endianness::MACHINE);
SerializeAdapter::serialize(&dipoleY, &serPtr, &dummy, maxSize,
SerializeIF::Endianness::MACHINE);
SerializeAdapter::serialize(&dipoleZ, &serPtr, &dummy, maxSize,
SerializeIF::Endianness::MACHINE);
SerializeAdapter::serialize(&torqueDuration, &serPtr, &dummy, maxSize,
SerializeIF::Endianness::MACHINE);
}
uint8_t* startOfActuateDataPtr() { return rawData + 2; }
int16_t* getDipoles() { return reinterpret_cast<int16_t*>(rawData + 2); }
uint16_t getTorqueDuration() {
uint8_t* data = rawData + 2 + 6;
uint16_t value = 0;
size_t dummy = 0;
SerializeAdapter::deSerialize(&value, data, &dummy, SerializeIF::Endianness::MACHINE);
return value;
}
void setSpecialRequest(imtq::SpecialRequest specialRequest) {
rawData[1] = static_cast<uint8_t>(specialRequest);
}
imtq::SpecialRequest getSpecialRequest() const {
return static_cast<imtq::SpecialRequest>(rawData[1]);
}
private:
ImtqRequest(uint8_t* rawData, size_t maxLen) : rawData(rawData) {
if (rawData != nullptr) {
rawData[0] = static_cast<uint8_t>(imtq::RequestType::MEASURE_NO_ACTUATION);
}
}
uint8_t* rawData;
size_t maxSize = 0;
};
struct ImtqRepliesDefault {
friend class ImtqPollingTask;
public:
static constexpr size_t BASE_LEN =
imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::SYSTEM_STATE + 1 +
+imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1 +
imtq::replySize::ENG_HK_DATA_REPLY + 1 + imtq::replySize::CAL_MTM_MEASUREMENT + 1;
ImtqRepliesDefault(const uint8_t* rawData) : rawData(const_cast<uint8_t*>(rawData)) {
initPointers();
}
uint8_t* getCalibMgmMeasurement() const { return calibMgmMeasurement + 1; }
bool wasCalibMgmMeasurementRead() const { return calibMgmMeasurement[0]; };
uint8_t* getEngHk() const { return engHk + 1; }
bool wasEngHkRead() const { return engHk[0]; };
uint8_t* getRawMgmMeasurement() const { return rawMgmMeasurement + 1; }
bool wasGetRawMgmMeasurementRead() const { return rawMgmMeasurement[0]; };
uint8_t* getSpecialRequest() const { return specialRequestReply + 1; }
bool wasSpecialRequestRead() const { return specialRequestReply[0]; }
uint8_t* getStartMtmMeasurement() const { return startMtmMeasurement + 1; }
bool wasStartMtmMeasurementRead() const { return startMtmMeasurement[0]; }
uint8_t* getSwReset() const { return swReset + 1; }
uint8_t* getSystemState() const { return systemState + 1; }
bool wasGetSystemStateRead() const { return systemState[0]; }
private:
void initPointers() {
swReset = rawData;
systemState = swReset + imtq::replySize::DEFAULT_MIN_LEN + 1;
startMtmMeasurement = systemState + imtq::replySize::SYSTEM_STATE + 1;
rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1;
engHk = rawMgmMeasurement + imtq::replySize::RAW_MTM_MEASUREMENT + 1;
calibMgmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1;
specialRequestReply = calibMgmMeasurement + imtq::replySize::CAL_MTM_MEASUREMENT + 1;
}
uint8_t* rawData;
uint8_t* swReset;
uint8_t* systemState;
uint8_t* startMtmMeasurement;
uint8_t* rawMgmMeasurement;
uint8_t* engHk;
uint8_t* calibMgmMeasurement;
// Share this to reduce amount of copied code for each transfer.
uint8_t* specialRequestReply;
};
struct ImtqRepliesWithTorque {
friend class ImtqPollingTask;
public:
static constexpr size_t BASE_LEN =
imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::ENG_HK_DATA_REPLY + 1 +
imtq::replySize::DEFAULT_MIN_LEN + 1 + imtq::replySize::RAW_MTM_MEASUREMENT + 1;
ImtqRepliesWithTorque(const uint8_t* rawData) : rawData(const_cast<uint8_t*>(rawData)) {
initPointers();
}
uint8_t* getDipoleActuation() const { return dipoleActuation + 1; }
bool wasDipoleActuationRead() const { return dipoleActuation[0]; }
uint8_t* getEngHk() const { return engHk + 1; }
bool wasEngHkRead() const { return engHk[0]; };
uint8_t* getRawMgmMeasurement() const { return rawMgmMeasurement + 1; }
bool wasGetRawMgmMeasurementRead() const { return rawMgmMeasurement[0]; };
private:
void initPointers() {
dipoleActuation = rawData;
engHk = dipoleActuation + imtq::replySize::DEFAULT_MIN_LEN + 1;
startMtmMeasurement = engHk + imtq::replySize::ENG_HK_DATA_REPLY + 1;
rawMgmMeasurement = startMtmMeasurement + imtq::replySize::DEFAULT_MIN_LEN + 1;
}
uint8_t* rawData;
uint8_t* dipoleActuation;
uint8_t* engHk;
uint8_t* startMtmMeasurement;
uint8_t* rawMgmMeasurement;
};
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ */

17
mission/tmtc/CfdpTmFunnel.cpp
View File

@ -59,16 +59,19 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
#endif
return result;
}
size_t serSize = 0;
result =
spacePacketHeader.serializeBe(&newPacketData, &serSize, spacePacketHeader.getFullPacketLen());
size_t packetLen = 0;
uint8_t* serPtr = newPacketData;
result = spacePacketHeader.serializeBe(&serPtr, &packetLen, spacePacketHeader.getFullPacketLen());
if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "CfdpTmFunnel::handlePacket: Error serializing packet" << std::endl;
#endif
return result;
}
std::memcpy(newPacketData, cfdpPacket, cfdpPacketLen);
std::memcpy(serPtr, cfdpPacket, cfdpPacketLen);
packetLen += cfdpPacketLen;
// Delete old packet
tmStore.deleteData(msg.getStorageId());
msg.setStorageId(newStoreId);
@ -80,14 +83,16 @@ ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
// Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
// to bother with send order and where the data is deleted.
store_address_t storeId;
result = tmStore.addData(&storeId, newPacketData, serSize);
result = tmStore.addData(&storeId, newPacketData, packetLen);
if (result == returnvalue::OK) {
msg.setStorageId(storeId);
} else {
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy or store "
"error"
<< std::endl;
#endif
break;
}
} else {
msg.setStorageId(origStoreId);