Merge branch 'main' into soc-calculator
This commit is contained in:
@ -169,7 +169,7 @@ void AcsController::performSafe() {
|
||||
guidance.getTargetParamsSafe(sunTargetDir);
|
||||
|
||||
double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
|
||||
uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
|
||||
gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
|
||||
acsParameters.safeModeControllerParameters.useMekf,
|
||||
@ -205,11 +205,13 @@ void AcsController::performSafe() {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
|
||||
*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipolMax);
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
||||
|
||||
// detumble check and switch
|
||||
if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf &&
|
||||
@ -231,8 +233,8 @@ void AcsController::performSafe() {
|
||||
}
|
||||
|
||||
updateCtrlValData(errAng, safeCtrlStrat);
|
||||
updateActuatorCmdData(cmdDipolMtqs);
|
||||
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
acsParameters.magnetorquerParameter.torqueDuration);
|
||||
}
|
||||
|
||||
@ -259,7 +261,7 @@ void AcsController::performDetumble() {
|
||||
triggerEvent(acs::MEKF_RECOVERY);
|
||||
mekfInvalidFlag = false;
|
||||
}
|
||||
uint8_t safeCtrlStrat = detumble.detumbleStrategy(
|
||||
acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
|
||||
mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
|
||||
mgmDataProcessed.mgmVecTotDerivative.isValid(),
|
||||
acsParameters.detumbleParameter.useFullDetumbleLaw);
|
||||
@ -279,11 +281,13 @@ void AcsController::performDetumble() {
|
||||
case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
|
||||
safeCtrlFailure(0, 1);
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs,
|
||||
*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipolMax);
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);
|
||||
|
||||
if (mekfData.satRotRateMekf.isValid() &&
|
||||
VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
|
||||
@ -304,8 +308,8 @@ void AcsController::performDetumble() {
|
||||
}
|
||||
|
||||
disableCtrlValData();
|
||||
updateActuatorCmdData(cmdDipolMtqs);
|
||||
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
updateActuatorCmdData(cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
acsParameters.magnetorquerParameter.torqueDuration);
|
||||
}
|
||||
|
||||
@ -366,24 +370,26 @@ void AcsController::performPointingCtrl() {
|
||||
// Variables required for setting actuators
|
||||
double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
|
||||
mgtDpDes[3] = {0, 0, 0};
|
||||
|
||||
switch (mode) {
|
||||
case acs::PTG_IDLE:
|
||||
guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
|
||||
guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
|
||||
targetSatRotRate);
|
||||
guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
|
||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&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, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&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);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
@ -397,17 +403,17 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
|
||||
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, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.targetModeControllerParameters, 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);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
@ -418,17 +424,17 @@ void AcsController::performPointingCtrl() {
|
||||
targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
|
||||
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, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.gsTargetModeControllerParameters, 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);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
|
||||
@ -442,63 +448,61 @@ void AcsController::performPointingCtrl() {
|
||||
errorSatRotRate, errorAngle);
|
||||
ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
|
||||
*rwPseudoInv, torquePtgRws);
|
||||
ptgCtrl.ptgNullspace(
|
||||
&acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
|
||||
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, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.nadirModeControllerParameters, 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);
|
||||
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:
|
||||
std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
|
||||
4 * sizeof(double));
|
||||
sizeof(targetQuat));
|
||||
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),
|
||||
&(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value),
|
||||
&(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
|
||||
ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
|
||||
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, acsParameters.rwHandlingParameters.maxTrq);
|
||||
ptgCtrl.ptgDesaturation(
|
||||
&acsParameters.inertialModeControllerParameters, 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);
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
||||
break;
|
||||
default:
|
||||
sif::error << "AcsController: Invalid mode for performPointingCtrl";
|
||||
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
actuatorCmd.cmdSpeedToRws(
|
||||
sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws,
|
||||
cmdSpeedRws, acsParameters.onBoardParams.sampleTime,
|
||||
acsParameters.rwHandlingParameters.maxRwSpeed,
|
||||
acsParameters.rwHandlingParameters.inertiaWheel);
|
||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
|
||||
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
|
||||
if (enableAntiStiction) {
|
||||
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
|
||||
}
|
||||
actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs,
|
||||
*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipolMax);
|
||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
||||
|
||||
updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
|
||||
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs);
|
||||
commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2],
|
||||
updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
|
||||
commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
|
||||
acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
|
||||
cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
|
||||
acsParameters.rwHandlingParameters.rampTime);
|
||||
|
||||
@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
|
||||
bool mekfLost = false;
|
||||
|
||||
int32_t cmdSpeedRws[4] = {0, 0, 0, 0};
|
||||
int16_t cmdDipolMtqs[3] = {0, 0, 0};
|
||||
int16_t cmdDipoleMtqs[3] = {0, 0, 0};
|
||||
|
||||
#if OBSW_THREAD_TRACING == 1
|
||||
uint32_t opCounter = 0;
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,7 +1,7 @@
|
||||
#ifndef MISSION_CONTROLLER_THERMALCONTROLLER_H_
|
||||
#define MISSION_CONTROLLER_THERMALCONTROLLER_H_
|
||||
|
||||
#include <bsp_q7s/core/CoreDefinitions.h>
|
||||
#include <bsp_q7s/core/defs.h>
|
||||
#include <fsfw/controller/ExtendedControllerBase.h>
|
||||
#include <fsfw/devicehandlers/DeviceHandlerThermalSet.h>
|
||||
#include <fsfw/timemanager/Countdown.h>
|
||||
@ -24,74 +24,7 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <list>
|
||||
|
||||
/**
|
||||
* NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit
|
||||
* is exceeded.
|
||||
* OP Limit: Soft limit. Device should be switched off or TCS controller should take action if the
|
||||
* limit is exceeded to avoid reaching NOP limit
|
||||
*/
|
||||
struct TempLimits {
|
||||
TempLimits(float nopLowerLimit, float opLowerLimit, float cutOffLimit, float opUpperLimit,
|
||||
float nopUpperLimit)
|
||||
: opLowerLimit(opLowerLimit),
|
||||
opUpperLimit(opUpperLimit),
|
||||
cutOffLimit(cutOffLimit),
|
||||
nopLowerLimit(nopLowerLimit),
|
||||
nopUpperLimit(nopUpperLimit) {}
|
||||
float opLowerLimit;
|
||||
float opUpperLimit;
|
||||
float cutOffLimit;
|
||||
float nopLowerLimit;
|
||||
float nopUpperLimit;
|
||||
};
|
||||
|
||||
struct ThermalState {
|
||||
uint8_t errorCounter;
|
||||
// Is heating on for that thermal module?
|
||||
bool heating = false;
|
||||
heater::Switch heaterSwitch = heater::Switch::NUMBER_OF_SWITCHES;
|
||||
// Heater start time and end times as UNIX seconds. Please note that these times will be updated
|
||||
// when a switch command is sent, with no guarantess that the heater actually went on.
|
||||
uint32_t heaterStartTime = 0;
|
||||
uint32_t heaterEndTime = 0;
|
||||
};
|
||||
|
||||
struct HeaterState {
|
||||
bool switchTransition;
|
||||
HeaterHandler::SwitchState target;
|
||||
uint8_t heaterSwitchControlCycles;
|
||||
};
|
||||
|
||||
using HeaterSwitchStates = std::array<HeaterHandler::SwitchState, heater::NUMBER_OF_SWITCHES>;
|
||||
|
||||
enum ThermalComponents : uint8_t {
|
||||
NONE = 0,
|
||||
ACS_BOARD = 1,
|
||||
MGT = 2,
|
||||
RW = 3,
|
||||
STR = 4,
|
||||
IF_BOARD = 5,
|
||||
TCS_BOARD = 6,
|
||||
OBC = 7,
|
||||
OBCIF_BOARD = 8,
|
||||
SBAND_TRANSCEIVER = 9,
|
||||
PCDUP60_BOARD = 10,
|
||||
PCDUACU = 11,
|
||||
PCDUPDU = 12,
|
||||
PLPCDU_BOARD = 13,
|
||||
PLOCMISSION_BOARD = 14,
|
||||
PLOCPROCESSING_BOARD = 15,
|
||||
DAC = 16,
|
||||
CAMERA = 17,
|
||||
DRO = 18,
|
||||
X8 = 19,
|
||||
HPA = 20,
|
||||
TX = 21,
|
||||
MPA = 22,
|
||||
SCEX_BOARD = 23,
|
||||
NUM_ENTRIES
|
||||
};
|
||||
#include <optional>
|
||||
|
||||
class ThermalController : public ExtendedControllerBase {
|
||||
public:
|
||||
@ -102,8 +35,28 @@ class ThermalController : public ExtendedControllerBase {
|
||||
static constexpr int16_t SANITY_LIMIT_LOWER_TEMP = -80;
|
||||
static constexpr int16_t SANITY_LIMIT_UPPER_TEMP = 160;
|
||||
|
||||
// 1 hour
|
||||
static constexpr uint32_t DEFAULT_MAX_HEATER_ON_DURATION_MS = 60 * 60 * 1000;
|
||||
static constexpr uint32_t MAX_HEATER_ON_DURATIONS_MS[8] = {// PLOC PROC board
|
||||
DEFAULT_MAX_HEATER_ON_DURATION_MS,
|
||||
// PCDU PDU
|
||||
DEFAULT_MAX_HEATER_ON_DURATION_MS,
|
||||
// ACS Board
|
||||
DEFAULT_MAX_HEATER_ON_DURATION_MS,
|
||||
// OBC Board
|
||||
DEFAULT_MAX_HEATER_ON_DURATION_MS,
|
||||
// Camera
|
||||
DEFAULT_MAX_HEATER_ON_DURATION_MS,
|
||||
// STR
|
||||
DEFAULT_MAX_HEATER_ON_DURATION_MS,
|
||||
// DRO
|
||||
DEFAULT_MAX_HEATER_ON_DURATION_MS,
|
||||
// S-Band
|
||||
DEFAULT_MAX_HEATER_ON_DURATION_MS};
|
||||
|
||||
ThermalController(object_id_t objectId, HeaterHandler& heater,
|
||||
const std::atomic_bool& tcsBoardShortUnavailable);
|
||||
const std::atomic_bool& tcsBoardShortUnavailable, bool pollPcdu1Tmp);
|
||||
virtual ~ThermalController();
|
||||
|
||||
ReturnValue_t initialize() override;
|
||||
|
||||
@ -111,16 +64,16 @@ class ThermalController : public ExtendedControllerBase {
|
||||
struct HeaterContext {
|
||||
public:
|
||||
HeaterContext(heater::Switch switchNr, heater::Switch redundantSwitchNr,
|
||||
const TempLimits& tempLimit)
|
||||
const tcsCtrl::TempLimits& tempLimit)
|
||||
: switchNr(switchNr), redSwitchNr(redundantSwitchNr), tempLimit(tempLimit) {}
|
||||
bool doHeaterHandling = true;
|
||||
heater::Switch switchNr;
|
||||
HeaterHandler::SwitchState switchState = HeaterHandler::SwitchState::OFF;
|
||||
heater::SwitchState switchState = heater::SwitchState::OFF;
|
||||
heater::Switch redSwitchNr;
|
||||
const TempLimits& tempLimit;
|
||||
const tcsCtrl::TempLimits& tempLimit;
|
||||
};
|
||||
|
||||
void performThermalModuleCtrl(const HeaterSwitchStates& heaterSwitchStates);
|
||||
void performThermalModuleCtrl(const tcsCtrl::HeaterSwitchStates& heaterSwitchStates);
|
||||
ReturnValue_t handleCommandMessage(CommandMessage* message) override;
|
||||
void performControlOperation() override;
|
||||
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
@ -142,12 +95,12 @@ class ThermalController : public ExtendedControllerBase {
|
||||
|
||||
HeaterHandler& heaterHandler;
|
||||
|
||||
bool pollPcdu1Tmp;
|
||||
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));
|
||||
tcsCtrl::TcsCtrlInfo tcsCtrlInfo;
|
||||
|
||||
DeviceHandlerThermalSet imtqThermalSet;
|
||||
|
||||
@ -173,7 +126,7 @@ class ThermalController : public ExtendedControllerBase {
|
||||
TMP1075::Tmp1075Dataset tmp1075SetTcs1;
|
||||
TMP1075::Tmp1075Dataset tmp1075SetPlPcdu0;
|
||||
// damaged
|
||||
// TMP1075::Tmp1075Dataset tmp1075SetPlPcdu1;
|
||||
TMP1075::Tmp1075Dataset* tmp1075SetPlPcdu1;
|
||||
TMP1075::Tmp1075Dataset tmp1075SetIfBoard;
|
||||
|
||||
// SUS
|
||||
@ -227,57 +180,67 @@ class ThermalController : public ExtendedControllerBase {
|
||||
lp_var_t<float> tempMgm2 =
|
||||
lp_var_t<float>(objects::MGM_2_LIS3_HANDLER, mgmLis3::TEMPERATURE_CELCIUS);
|
||||
lp_var_t<float> tempAdcPayloadPcdu = lp_var_t<float>(objects::PLPCDU_HANDLER, plpcdu::TEMP);
|
||||
lp_vec_t<int16_t, 9> currentVecPdu2 =
|
||||
lp_vec_t<int16_t, 9>(gp_id_t(objects::PDU2_HANDLER, PDU::pool::PDU_CURRENTS));
|
||||
|
||||
// TempLimits
|
||||
TempLimits acsBoardLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
|
||||
TempLimits mgtLimits = TempLimits(-40.0, -40.0, 65.0, 70.0, 70.0);
|
||||
TempLimits rwLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
|
||||
TempLimits strLimits = TempLimits(-30.0, -20.0, 65.0, 70.0, 80.0);
|
||||
TempLimits ifBoardLimits = TempLimits(-65.0, -40.0, 80.0, 85.0, 150.0);
|
||||
TempLimits tcsBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 130.0);
|
||||
TempLimits obcLimits = TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
|
||||
TempLimits obcIfBoardLimits = TempLimits(-65.0, -40.0, 80.0, 85.0, 125.0);
|
||||
TempLimits sBandTransceiverLimits = TempLimits(-40.0, -25.0, 35.0, 40.0, 65.0);
|
||||
TempLimits pcduP60BoardLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
|
||||
TempLimits pcduAcuLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
|
||||
TempLimits pcduPduLimits = TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
|
||||
TempLimits plPcduBoardLimits = TempLimits(-55.0, -40.0, 80.0, 85.0, 125.0);
|
||||
TempLimits plocMissionBoardLimits = TempLimits(-30.0, -10.0, 40.0, 45.0, 60);
|
||||
TempLimits plocProcessingBoardLimits = TempLimits(-30.0, -10.0, 40.0, 45.0, 60.0);
|
||||
TempLimits dacLimits = TempLimits(-65.0, -40.0, 113.0, 118.0, 150.0);
|
||||
TempLimits cameraLimits = TempLimits(-40.0, -30.0, 60.0, 65.0, 85.0);
|
||||
TempLimits droLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
TempLimits x8Limits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
TempLimits hpaLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
TempLimits txLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
TempLimits mpaLimits = TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
TempLimits scexBoardLimits = TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0);
|
||||
tcsCtrl::TempLimits acsBoardLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
|
||||
tcsCtrl::TempLimits mgtLimits = tcsCtrl::TempLimits(-40.0, -40.0, 65.0, 70.0, 70.0);
|
||||
tcsCtrl::TempLimits rwLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
|
||||
tcsCtrl::TempLimits strLimits = tcsCtrl::TempLimits(-30.0, -20.0, 65.0, 70.0, 80.0);
|
||||
tcsCtrl::TempLimits ifBoardLimits = tcsCtrl::TempLimits(-65.0, -40.0, 80.0, 85.0, 150.0);
|
||||
tcsCtrl::TempLimits tcsBoardLimits = tcsCtrl::TempLimits(-60.0, -40.0, 80.0, 85.0, 130.0);
|
||||
tcsCtrl::TempLimits obcLimits = tcsCtrl::TempLimits(-40.0, -40.0, 80.0, 85.0, 85.0);
|
||||
tcsCtrl::TempLimits obcIfBoardLimits = tcsCtrl::TempLimits(-65.0, -40.0, 80.0, 85.0, 125.0);
|
||||
tcsCtrl::TempLimits sBandTransceiverLimits = tcsCtrl::TempLimits(-40.0, -25.0, 35.0, 40.0, 65.0);
|
||||
tcsCtrl::TempLimits pcduP60BoardLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
|
||||
tcsCtrl::TempLimits pcduAcuLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
|
||||
tcsCtrl::TempLimits pcduPduLimits = tcsCtrl::TempLimits(-35.0, -35.0, 80.0, 85.0, 85.0);
|
||||
tcsCtrl::TempLimits plPcduBoardLimits = tcsCtrl::TempLimits(-55.0, -40.0, 80.0, 85.0, 125.0);
|
||||
tcsCtrl::TempLimits plocMissionBoardLimits = tcsCtrl::TempLimits(-30.0, -10.0, 40.0, 45.0, 60);
|
||||
tcsCtrl::TempLimits plocProcessingBoardLimits =
|
||||
tcsCtrl::TempLimits(-30.0, -10.0, 40.0, 45.0, 60.0);
|
||||
tcsCtrl::TempLimits dacLimits = tcsCtrl::TempLimits(-65.0, -40.0, 113.0, 118.0, 150.0);
|
||||
tcsCtrl::TempLimits cameraLimits = tcsCtrl::TempLimits(-40.0, -30.0, 60.0, 65.0, 85.0);
|
||||
tcsCtrl::TempLimits droLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
tcsCtrl::TempLimits x8Limits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
tcsCtrl::TempLimits hpaLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
tcsCtrl::TempLimits txLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
tcsCtrl::TempLimits mpaLimits = tcsCtrl::TempLimits(-40.0, -30.0, 75.0, 80.0, 90.0);
|
||||
tcsCtrl::TempLimits scexBoardLimits = tcsCtrl::TempLimits(-60.0, -40.0, 80.0, 85.0, 150.0);
|
||||
|
||||
struct CtrlContext {
|
||||
double sensorTemp = INVALID_TEMPERATURE;
|
||||
uint8_t currentSensorIndex = 0;
|
||||
tcsCtrl::ThermalComponents thermalComponent = tcsCtrl::NONE;
|
||||
bool redSwitchNrInUse = false;
|
||||
bool componentAboveCutOffLimit = false;
|
||||
bool componentAboveUpperLimit = false;
|
||||
Event overHeatEventToTrigger;
|
||||
} ctrlCtx;
|
||||
|
||||
double sensorTemp = INVALID_TEMPERATURE;
|
||||
ThermalComponents thermalComponent = NONE;
|
||||
bool redSwitchNrInUse = false;
|
||||
MessageQueueId_t camId = MessageQueueIF::NO_QUEUE;
|
||||
bool componentAboveCutOffLimit = false;
|
||||
bool componentAboveUpperLimit = false;
|
||||
Event overHeatEventToTrigger;
|
||||
bool eBandTooHotFlag = false;
|
||||
bool camTooHotOneShotFlag = false;
|
||||
bool scexTooHotFlag = false;
|
||||
bool plocTooHotFlag = false;
|
||||
bool pcduSystemTooHotFlag = false;
|
||||
bool syrlinksTooHotFlag = false;
|
||||
bool obcTooHotFlag = false;
|
||||
bool mgtTooHotFlag = false;
|
||||
bool strTooHotFlag = false;
|
||||
bool rwTooHotFlag = false;
|
||||
|
||||
struct TooHotFlags {
|
||||
bool eBandTooHotFlag = false;
|
||||
bool camTooHotOneShotFlag = false;
|
||||
bool scexTooHotFlag = false;
|
||||
bool plocTooHotFlag = false;
|
||||
bool pcduSystemTooHotFlag = false;
|
||||
bool syrlinksTooHotFlag = false;
|
||||
bool obcTooHotFlag = false;
|
||||
bool mgtTooHotFlag = false;
|
||||
bool strTooHotFlag = false;
|
||||
bool rwTooHotFlag = false;
|
||||
} tooHotFlags;
|
||||
|
||||
bool transitionWhenHeatersOff = false;
|
||||
uint32_t transitionWhenHeatersOffCycles = 0;
|
||||
Mode_t targetMode = MODE_OFF;
|
||||
Submode_t targetSubmode = SUBMODE_NONE;
|
||||
uint32_t cycles = 0;
|
||||
std::array<ThermalState, ThermalComponents::NUM_ENTRIES> thermalStates{};
|
||||
std::array<HeaterState, heater::NUMBER_OF_SWITCHES> heaterStates{};
|
||||
std::array<tcsCtrl::ThermalState, tcsCtrl::NUM_THERMAL_COMPONENTS> thermalStates{};
|
||||
std::array<tcsCtrl::HeaterState, heater::NUMBER_OF_SWITCHES> heaterStates{};
|
||||
|
||||
// Initial delay to make sure all pool variables have been initialized their owners.
|
||||
// Also, wait for system initialization to complete.
|
||||
@ -298,6 +261,12 @@ class ThermalController : public ExtendedControllerBase {
|
||||
PoolEntry<uint8_t> heaterSwitchStates = PoolEntry<uint8_t>(heater::NUMBER_OF_SWITCHES);
|
||||
PoolEntry<int16_t> heaterCurrent = PoolEntry<int16_t>();
|
||||
|
||||
PoolEntry<uint8_t> tcsCtrlHeaterOn = PoolEntry<uint8_t>(tcsCtrl::NUM_THERMAL_COMPONENTS);
|
||||
PoolEntry<uint8_t> tcsCtrlSensorIdx = PoolEntry<uint8_t>(tcsCtrl::NUM_THERMAL_COMPONENTS);
|
||||
PoolEntry<uint8_t> tcsCtrlHeaterIdx = PoolEntry<uint8_t>(tcsCtrl::NUM_THERMAL_COMPONENTS);
|
||||
PoolEntry<uint32_t> tcsCtrlStartTimes = PoolEntry<uint32_t>(tcsCtrl::NUM_THERMAL_COMPONENTS);
|
||||
PoolEntry<uint32_t> tcsCtrlEndTimes = PoolEntry<uint32_t>(tcsCtrl::NUM_THERMAL_COMPONENTS);
|
||||
|
||||
static constexpr dur_millis_t MUTEX_TIMEOUT = 50;
|
||||
|
||||
void startTransition(Mode_t mode, Submode_t submode) override;
|
||||
@ -319,8 +288,8 @@ class ThermalController : public ExtendedControllerBase {
|
||||
bool selectAndReadSensorTemp(HeaterContext& htrCtx);
|
||||
|
||||
void heaterSwitchHelperAllOff();
|
||||
void heaterSwitchHelper(heater::Switch switchNr, HeaterHandler::SwitchState state,
|
||||
unsigned componentIdx);
|
||||
void heaterSwitchHelper(heater::Switch switchNr, heater::SwitchState state,
|
||||
std::optional<unsigned> componentIdx);
|
||||
|
||||
void ctrlAcsBoard();
|
||||
void ctrlMgt();
|
||||
@ -329,7 +298,6 @@ class ThermalController : public ExtendedControllerBase {
|
||||
void ctrlIfBoard();
|
||||
void ctrlTcsBoard();
|
||||
void ctrlObc();
|
||||
void ctrlObcIfBoard();
|
||||
void ctrlSBandTransceiver();
|
||||
void ctrlPcduP60Board();
|
||||
void ctrlPcduAcu();
|
||||
@ -345,7 +313,22 @@ class ThermalController : public ExtendedControllerBase {
|
||||
void ctrlTx();
|
||||
void ctrlMpa();
|
||||
void ctrlScexBoard();
|
||||
void heaterTransitionControl(const HeaterSwitchStates& currentHeaterStates);
|
||||
|
||||
/**
|
||||
* The transition of heaters might take some time. As long as a transition is
|
||||
* going on, the TCS controller works in a reduced form. This function takes care
|
||||
* of tracking transition and capturing their completion.
|
||||
* @param currentHeaterStates
|
||||
*/
|
||||
void heaterTransitionControl(const tcsCtrl::HeaterSwitchStates& currentHeaterStates);
|
||||
/**
|
||||
* Control tasks to prevent heaters being on for prolonged periods. Ideally, this
|
||||
* should never happen, but this task prevents bugs from causing heaters to stay on
|
||||
* for a long time, which draws a lot of power.
|
||||
* @param currentHeaterStates
|
||||
*/
|
||||
void heaterMaxDurationControl(const tcsCtrl::HeaterSwitchStates& currentHeaterStates);
|
||||
void crossCheckHeaterStateOfComponentsWhenHeaterGoesOff(heater::Switch switchIdx);
|
||||
void setMode(Mode_t mode, Submode_t submode);
|
||||
uint32_t tempFloatToU32() const;
|
||||
bool tooHotHandler(object_id_t object, bool& oneShotFlag);
|
||||
|
||||
@ -221,6 +221,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
case 0x23:
|
||||
parameterWrapper->setMatrix(susHandlingParameters.sus11coeffBeta);
|
||||
break;
|
||||
case 0x24:
|
||||
parameterWrapper->set(susHandlingParameters.susBrightnessThreshold);
|
||||
break;
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
}
|
||||
@ -318,7 +321,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->setMatrix(rwMatrices.without4);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->setVector(rwMatrices.nullspace);
|
||||
parameterWrapper->setVector(rwMatrices.nullspaceVector);
|
||||
break;
|
||||
default:
|
||||
return INVALID_IDENTIFIER_ID;
|
||||
@ -378,15 +381,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(idleModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(idleModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(idleModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(idleModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
default:
|
||||
@ -411,48 +417,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(targetModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(targetModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
|
||||
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
|
||||
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
|
||||
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xC:
|
||||
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
|
||||
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
|
||||
break;
|
||||
case 0xD:
|
||||
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
|
||||
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
case 0xE:
|
||||
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
|
||||
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
|
||||
break;
|
||||
case 0xF:
|
||||
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
|
||||
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
|
||||
break;
|
||||
case 0x10:
|
||||
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
|
||||
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
|
||||
break;
|
||||
case 0x11:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
|
||||
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
|
||||
break;
|
||||
case 0x12:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
|
||||
break;
|
||||
case 0x13:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
|
||||
break;
|
||||
case 0x14:
|
||||
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
|
||||
break;
|
||||
default:
|
||||
@ -477,30 +486,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
|
||||
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
case 0xC:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
|
||||
break;
|
||||
case 0xD:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
|
||||
break;
|
||||
case 0xE:
|
||||
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
|
||||
break;
|
||||
default:
|
||||
@ -525,27 +537,30 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(nadirModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(nadirModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
|
||||
parameterWrapper->set(nadirModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
|
||||
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
|
||||
break;
|
||||
case 0xC:
|
||||
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xD:
|
||||
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
|
||||
break;
|
||||
default:
|
||||
@ -570,21 +585,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->set(inertialModeControllerParameters.gainNullspace);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
|
||||
parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef);
|
||||
break;
|
||||
case 0x7:
|
||||
parameterWrapper->set(inertialModeControllerParameters.desatOn);
|
||||
parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor);
|
||||
break;
|
||||
case 0x8:
|
||||
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
|
||||
parameterWrapper->set(inertialModeControllerParameters.desatOn);
|
||||
break;
|
||||
case 0x9:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
|
||||
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
|
||||
break;
|
||||
case 0xA:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
|
||||
break;
|
||||
case 0xB:
|
||||
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
|
||||
break;
|
||||
case 0xC:
|
||||
@ -696,7 +714,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
||||
parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment);
|
||||
break;
|
||||
case 0x5:
|
||||
parameterWrapper->set(magnetorquerParameter.dipolMax);
|
||||
parameterWrapper->set(magnetorquerParameter.dipoleMax);
|
||||
break;
|
||||
case 0x6:
|
||||
parameterWrapper->set(magnetorquerParameter.torqueDuration);
|
||||
|
||||
@ -766,6 +766,7 @@ class AcsParameters : public HasParametersIF {
|
||||
{116.975421945286, -5.53022680362263, -5.61081660666997, 0.109754904982136,
|
||||
0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513,
|
||||
-0.000889232196185857, -0.00168429567131815}};
|
||||
float susBrightnessThreshold = 0.7;
|
||||
} susHandlingParameters;
|
||||
|
||||
struct GyrHandlingParameters {
|
||||
@ -781,9 +782,9 @@ class AcsParameters : public HasParametersIF {
|
||||
|
||||
/* var = sigma^2, 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 */
|
||||
float gyr02variance[3] = {pow(3.0e-3, 2), // RND_x = 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, 2)}; // RND_z = 4.3e-3 deg/s/sqrt(Hz) rms
|
||||
float gyr02variance[3] = {pow(4.6e-3, 2), // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
|
||||
pow(4.6e-3, 2), // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
|
||||
pow(6.1e-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)};
|
||||
uint8_t preferAdis = false;
|
||||
float gyrFilterWeight = 0.6;
|
||||
@ -816,7 +817,7 @@ class AcsParameters : public HasParametersIF {
|
||||
{1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}};
|
||||
double without4[4][3] = {
|
||||
{0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}};
|
||||
double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000};
|
||||
double nullspaceVector[4] = {-1, 1, -1, 1};
|
||||
} rwMatrices;
|
||||
|
||||
struct SafeModeControllerParameters {
|
||||
@ -840,7 +841,9 @@ class AcsParameters : public HasParametersIF {
|
||||
double om = 0.3;
|
||||
double omMax = 1 * M_PI / 180;
|
||||
double qiMin = 0.1;
|
||||
|
||||
double gainNullspace = 0.01;
|
||||
double nullspaceSpeed = 32500; // 0.1 RPM
|
||||
|
||||
double desatMomentumRef[3] = {0, 0, 0};
|
||||
double deSatGainFactor = 1000;
|
||||
@ -933,7 +936,7 @@ class AcsParameters : public HasParametersIF {
|
||||
double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}};
|
||||
double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
|
||||
double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}};
|
||||
double dipolMax = 0.2; // [Am^2]
|
||||
double dipoleMax = 0.2; // [Am^2]
|
||||
|
||||
uint16_t torqueDuration = 300; // [ms]
|
||||
} magnetorquerParameter;
|
||||
|
||||
@ -5,11 +5,6 @@
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "util/CholeskyDecomposition.h"
|
||||
#include "util/MathOperations.h"
|
||||
|
||||
ActuatorCmd::ActuatorCmd() {}
|
||||
|
||||
ActuatorCmd::~ActuatorCmd() {}
|
||||
@ -25,24 +20,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) {
|
||||
}
|
||||
}
|
||||
|
||||
void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2,
|
||||
int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed,
|
||||
double sampleTime, int32_t maxRwSpeed, double inertiaWheel) {
|
||||
using namespace Math;
|
||||
|
||||
// Calculating the commanded speed in RPM for every reaction wheel
|
||||
void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1,
|
||||
const int32_t speedRw2, const int32_t speedRw3,
|
||||
const double sampleTime, const double inertiaWheel,
|
||||
const int32_t maxRwSpeed, const double *rwTorque,
|
||||
int32_t *rwCmdSpeed) {
|
||||
// group RW speed values (in 0.1 [RPM]) in vector
|
||||
int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3};
|
||||
|
||||
// calculate required RW speed as sum of current RW speed and RW speed delta
|
||||
// delta w_rw = delta t / I_RW * torque_RW [rad/s]
|
||||
double deltaSpeed[4] = {0, 0, 0, 0};
|
||||
double radToRpm = 60 / (2 * PI); // factor for conversion to RPM
|
||||
// W_RW = Torque_RW / I_RW * delta t [rad/s]
|
||||
double factor = sampleTime / inertiaWheel * radToRpm;
|
||||
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
|
||||
const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10;
|
||||
VectorOperations<double>::mulScalar(rwTorque, factor, deltaSpeed, 4);
|
||||
|
||||
// convert double to int32
|
||||
int32_t deltaSpeedInt[4] = {0, 0, 0, 0};
|
||||
for (int i = 0; i < 4; i++) {
|
||||
deltaSpeedInt[i] = std::round(deltaSpeed[i]);
|
||||
}
|
||||
|
||||
// sum of current RW speed and RW speed delta
|
||||
VectorOperations<int32_t>::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4);
|
||||
VectorOperations<int32_t>::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4);
|
||||
|
||||
// crop values which would exceed the maximum possible RPM
|
||||
for (uint8_t i = 0; i < 4; i++) {
|
||||
if (rwCmdSpeed[i] > maxRwSpeed) {
|
||||
rwCmdSpeed[i] = maxRwSpeed;
|
||||
@ -52,24 +53,25 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee
|
||||
}
|
||||
}
|
||||
|
||||
void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
|
||||
const double *inverseAlignment, double maxDipol) {
|
||||
// Convert to actuator frame
|
||||
double dipolMomentActuatorDouble[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3,
|
||||
1);
|
||||
// Scaling along largest element if dipol exceeds maximum
|
||||
void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
|
||||
const double *dipoleMoment, int16_t *dipoleMomentActuator) {
|
||||
// convert to actuator frame
|
||||
double dipoleMomentActuatorDouble[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3,
|
||||
3, 1);
|
||||
// scaling along largest element if dipole exceeds maximum
|
||||
uint8_t maxIdx = 0;
|
||||
VectorOperations<double>::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx);
|
||||
double maxAbsValue = std::abs(dipolMomentActuatorDouble[maxIdx]);
|
||||
if (maxAbsValue > maxDipol) {
|
||||
double scalingFactor = maxDipol / maxAbsValue;
|
||||
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, scalingFactor,
|
||||
dipolMomentActuatorDouble, 3);
|
||||
VectorOperations<double>::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx);
|
||||
double maxAbsValue = std::abs(dipoleMomentActuatorDouble[maxIdx]);
|
||||
if (maxAbsValue > maxDipole) {
|
||||
double scalingFactor = maxDipole / maxAbsValue;
|
||||
VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, scalingFactor,
|
||||
dipoleMomentActuatorDouble, 3);
|
||||
}
|
||||
// scale dipole from 1 Am^2 to 1e^-4 Am^2
|
||||
VectorOperations<double>::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3);
|
||||
VectorOperations<double>::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble,
|
||||
3);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]);
|
||||
dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,9 +1,7 @@
|
||||
#ifndef ACTUATORCMD_H_
|
||||
#define ACTUATORCMD_H_
|
||||
|
||||
#include "MultiplicativeKalmanFilter.h"
|
||||
#include "SensorProcessing.h"
|
||||
#include "SensorValues.h"
|
||||
#include <cmath>
|
||||
|
||||
class ActuatorCmd {
|
||||
public:
|
||||
@ -19,29 +17,30 @@ class ActuatorCmd {
|
||||
void scalingTorqueRws(double *rwTrq, double maxTorque);
|
||||
|
||||
/*
|
||||
* @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction
|
||||
* wheels, also will calculate the needed revolutions per minute for the RWs, which will be given
|
||||
* as Input to the RWs
|
||||
* @param: rwTrqIn given torque from pointing controller
|
||||
* rwTrqNS Nullspace torque
|
||||
* @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration,
|
||||
* given the required torque calculated by the controller. Will also scale down the RPM of the
|
||||
* wheels if they exceed the maximum possible RPM
|
||||
* @param: rwTrq given torque from pointing controller
|
||||
* rwCmdSpeed output revolutions per minute for every
|
||||
* reaction wheel
|
||||
*/
|
||||
void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3,
|
||||
const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime,
|
||||
int32_t maxRwSpeed, double inertiaWheel);
|
||||
void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
|
||||
const int32_t speedRw3, const double sampleTime, const double inertiaWheel,
|
||||
const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed);
|
||||
|
||||
/*
|
||||
* @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques
|
||||
* @brief: cmdDipoleMtq() gives the commanded dipole moment for the
|
||||
* magnetorquer
|
||||
*
|
||||
* @param: dipolMoment given dipol moment in spacecraft frame
|
||||
* dipolMomentActuator resulting dipol moment in actuator reference frame
|
||||
* @param: dipoleMoment given dipole moment in spacecraft frame
|
||||
* dipoleMomentActuator resulting dipole moment in actuator reference frame
|
||||
*/
|
||||
void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator,
|
||||
const double *inverseAlignment, double maxDipol);
|
||||
void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole,
|
||||
const double *dipoleMoment, int16_t *dipoleMomentActuator);
|
||||
|
||||
protected:
|
||||
private:
|
||||
static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI);
|
||||
};
|
||||
|
||||
#endif /* ACTUATORCMD_H_ */
|
||||
|
||||
@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3]
|
||||
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) {
|
||||
void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]) {
|
||||
//-------------------------------------------------------------------------------------
|
||||
// Calculation of target quaternion to sun
|
||||
//-------------------------------------------------------------------------------------
|
||||
@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double
|
||||
//----------------------------------------------------------------------------
|
||||
// Calculation of reference rotation rate
|
||||
//----------------------------------------------------------------------------
|
||||
refSatRate[0] = 0;
|
||||
refSatRate[1] = 0;
|
||||
refSatRate[2] = 0;
|
||||
int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax;
|
||||
targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate);
|
||||
}
|
||||
|
||||
void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4],
|
||||
@ -412,7 +412,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl
|
||||
|
||||
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) {
|
||||
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
|
||||
@ -424,26 +424,17 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
||||
// 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 ??
|
||||
// Calculate error satellite rotational rate
|
||||
// First combine the target and reference satellite rotational rates
|
||||
double combinedRefSatRotRate[3] = {0, 0, 0};
|
||||
VectorOperations<double>::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3);
|
||||
// Then subtract the combined required satellite rotational rates from the actual rate
|
||||
VectorOperations<double>::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3);
|
||||
}
|
||||
|
||||
void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4],
|
||||
double targetSatRotRate[3], double errorQuat[4],
|
||||
double errorSatRotRate[3], double errorAngle) {
|
||||
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
|
||||
@ -453,17 +444,8 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
|
||||
// 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 ??
|
||||
// Calculate error satellite rotational rate
|
||||
VectorOperations<double>::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3);
|
||||
}
|
||||
|
||||
void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
@ -471,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua
|
||||
//-------------------------------------------------------------------------------------
|
||||
// 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));
|
||||
double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 -
|
||||
(timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6);
|
||||
if (VectorOperations<double>::norm(savedQuaternion, 4) == 0) {
|
||||
std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion));
|
||||
}
|
||||
if (timeElapsed < timeElapsedMax) {
|
||||
double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0};
|
||||
QuaternionOperations::inverse(quatInertialTarget, q);
|
||||
QuaternionOperations::inverse(savedQuaternion, qS);
|
||||
double qDiff[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(quatInertialTarget, savedQuaternion, qDiff, 4);
|
||||
VectorOperations<double>::subtract(q, qS, qDiff, 4);
|
||||
VectorOperations<double>::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4);
|
||||
|
||||
double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]},
|
||||
qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
|
||||
double tgtQuatVec[3] = {q[0], q[1], q[2]};
|
||||
double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]};
|
||||
double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(quatInertialTarget, qDiff, sum1);
|
||||
VectorOperations<double>::cross(tgtQuatVec, qDiffVec, sum1);
|
||||
VectorOperations<double>::mulScalar(tgtQuatVec, qDiff[3], sum2, 3);
|
||||
VectorOperations<double>::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3);
|
||||
VectorOperations<double>::mulScalar(qDiffVec, q[3], sum3, 3);
|
||||
VectorOperations<double>::add(sum1, sum2, sum, 3);
|
||||
VectorOperations<double>::subtract(sum, sum3, sum, 3);
|
||||
double omegaRefNew[3] = {0, 0, 0};
|
||||
@ -531,10 +518,6 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues,
|
||||
std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double));
|
||||
return returnvalue::OK;
|
||||
} else {
|
||||
// @note: This one takes the normal pseudoInverse of all four raction wheels valid.
|
||||
// Does not make sense, but is implemented that way in MATLAB ?!
|
||||
// Thought: It does not really play a role, because in case there are more then one
|
||||
// reaction wheel invalid the pointing control is destined to fail.
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
@ -15,7 +15,7 @@ class Guidance {
|
||||
void getTargetParamsSafe(double sunTargetSafe[3]);
|
||||
ReturnValue_t solarArrayDeploymentComplete();
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position and
|
||||
// Function to get the target quaternion and reference rotation rate from gps position and
|
||||
// position of the ground station
|
||||
void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3],
|
||||
double refDirB[3], double quatBI[4], double targetQuat[4],
|
||||
@ -25,9 +25,10 @@ class Guidance {
|
||||
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
|
||||
// Function to get the target quaternion and reference rotation rate for sun pointing after ground
|
||||
// station
|
||||
void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]);
|
||||
void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4],
|
||||
double targetSatRotRate[3]);
|
||||
|
||||
// Function to get the target quaternion and refence rotation rate from gps position for Nadir
|
||||
// pointing
|
||||
@ -37,15 +38,15 @@ class Guidance {
|
||||
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
|
||||
// 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);
|
||||
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);
|
||||
double &errorAngle);
|
||||
|
||||
void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4],
|
||||
double *targetSatRotRate);
|
||||
|
||||
@ -30,10 +30,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
// ------------------------------------------------
|
||||
double magIgrfModel[3] = {0.0, 0.0, 0.0};
|
||||
if (gpsValid) {
|
||||
// Should be existing class object which will be called and modified here.
|
||||
Igrf13Model igrf13;
|
||||
// So the line above should not be done here. Update: Can be done here as long updated coffs
|
||||
// stored in acsParameters ?
|
||||
igrf13.schmidtNormalization();
|
||||
igrf13.updateCoeffGH(timeOfMgmMeasurement);
|
||||
// maybe put a condition here, to only update after a full day, this
|
||||
@ -45,14 +42,13 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
|
||||
{
|
||||
PoolReadGuard pg(mgmDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
float zeroVec[3] = {0.0, 0.0, 0.0};
|
||||
std::memcpy(mgmDataProcessed->mgm0vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm1vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm2vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm3vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm4vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgmVecTot.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm0vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm1vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm2vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm3vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgm4vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(mgmDataProcessed->mgmVecTot.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
std::memcpy(mgmDataProcessed->mgmVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
mgmDataProcessed->setValidity(false, true);
|
||||
std::memcpy(mgmDataProcessed->magIgrfModel.value, magIgrfModel, 3 * sizeof(double));
|
||||
mgmDataProcessed->magIgrfModel.setValid(gpsValid);
|
||||
@ -210,63 +206,68 @@ void SensorProcessing::processSus(
|
||||
sunIjkModel[0] = cos(eclipticLongitude);
|
||||
sunIjkModel[1] = sin(eclipticLongitude) * cos(epsilon);
|
||||
sunIjkModel[2] = sin(eclipticLongitude) * sin(epsilon);
|
||||
|
||||
uint64_t susBrightness[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
|
||||
if (sus0valid) {
|
||||
sus0valid = susConverter.checkSunSensorData(sus0Value);
|
||||
susBrightness[0] = susConverter.checkSunSensorData(sus0Value);
|
||||
}
|
||||
if (sus1valid) {
|
||||
sus1valid = susConverter.checkSunSensorData(sus1Value);
|
||||
susBrightness[1] = susConverter.checkSunSensorData(sus1Value);
|
||||
}
|
||||
if (sus2valid) {
|
||||
sus2valid = susConverter.checkSunSensorData(sus2Value);
|
||||
susBrightness[2] = susConverter.checkSunSensorData(sus2Value);
|
||||
}
|
||||
if (sus3valid) {
|
||||
sus3valid = susConverter.checkSunSensorData(sus3Value);
|
||||
susBrightness[3] = susConverter.checkSunSensorData(sus3Value);
|
||||
}
|
||||
if (sus4valid) {
|
||||
sus4valid = susConverter.checkSunSensorData(sus4Value);
|
||||
susBrightness[4] = susConverter.checkSunSensorData(sus4Value);
|
||||
}
|
||||
if (sus5valid) {
|
||||
sus5valid = susConverter.checkSunSensorData(sus5Value);
|
||||
susBrightness[5] = susConverter.checkSunSensorData(sus5Value);
|
||||
}
|
||||
if (sus6valid) {
|
||||
sus6valid = susConverter.checkSunSensorData(sus6Value);
|
||||
susBrightness[6] = susConverter.checkSunSensorData(sus6Value);
|
||||
}
|
||||
if (sus7valid) {
|
||||
sus7valid = susConverter.checkSunSensorData(sus7Value);
|
||||
susBrightness[7] = susConverter.checkSunSensorData(sus7Value);
|
||||
}
|
||||
if (sus8valid) {
|
||||
sus8valid = susConverter.checkSunSensorData(sus8Value);
|
||||
susBrightness[8] = susConverter.checkSunSensorData(sus8Value);
|
||||
}
|
||||
if (sus9valid) {
|
||||
sus9valid = susConverter.checkSunSensorData(sus9Value);
|
||||
susBrightness[9] = susConverter.checkSunSensorData(sus9Value);
|
||||
}
|
||||
if (sus10valid) {
|
||||
sus10valid = susConverter.checkSunSensorData(sus10Value);
|
||||
susBrightness[10] = susConverter.checkSunSensorData(sus10Value);
|
||||
}
|
||||
if (sus11valid) {
|
||||
sus11valid = susConverter.checkSunSensorData(sus11Value);
|
||||
susBrightness[11] = susConverter.checkSunSensorData(sus11Value);
|
||||
}
|
||||
|
||||
if (!sus0valid && !sus1valid && !sus2valid && !sus3valid && !sus4valid && !sus5valid &&
|
||||
!sus6valid && !sus7valid && !sus8valid && !sus9valid && !sus10valid && !sus11valid) {
|
||||
bool susValid[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
|
||||
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
|
||||
bool allInvalid =
|
||||
susConverter.checkValidity(susValid, susBrightness, susParameters->susBrightnessThreshold);
|
||||
|
||||
if (allInvalid) {
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
float zeroVec[3] = {0.0, 0.0, 0.0};
|
||||
std::memcpy(susDataProcessed->sus0vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus1vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus2vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus3vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus4vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus5vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus6vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus7vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus8vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus9vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus10vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus11vec.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->susVecTot.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->susVecTotDerivative.value, zeroVec, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus0vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus1vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus2vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus3vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus4vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus5vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus6vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus7vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus8vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus9vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus10vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus11vec.value, ZERO_VEC_F, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->susVecTot.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
std::memcpy(susDataProcessed->susVecTotDerivative.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
susDataProcessed->setValidity(false, true);
|
||||
std::memcpy(susDataProcessed->sunIjkModel.value, sunIjkModel, 3 * sizeof(double));
|
||||
susDataProcessed->sunIjkModel.setValid(true);
|
||||
@ -274,118 +275,78 @@ void SensorProcessing::processSus(
|
||||
}
|
||||
return;
|
||||
}
|
||||
// WARNING: NOT TRANSFORMED IN BODY FRAME YET
|
||||
// Transformation into Geomtry Frame
|
||||
float sus0VecBody[3] = {0, 0, 0}, sus1VecBody[3] = {0, 0, 0}, sus2VecBody[3] = {0, 0, 0},
|
||||
sus3VecBody[3] = {0, 0, 0}, sus4VecBody[3] = {0, 0, 0}, sus5VecBody[3] = {0, 0, 0},
|
||||
sus6VecBody[3] = {0, 0, 0}, sus7VecBody[3] = {0, 0, 0}, sus8VecBody[3] = {0, 0, 0},
|
||||
sus9VecBody[3] = {0, 0, 0}, sus10VecBody[3] = {0, 0, 0}, sus11VecBody[3] = {0, 0, 0};
|
||||
|
||||
if (sus0valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus0orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus0Value, susParameters->sus0coeffAlpha,
|
||||
susParameters->sus0coeffBeta),
|
||||
sus0VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus1valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus1orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus1Value, susParameters->sus1coeffAlpha,
|
||||
susParameters->sus1coeffBeta),
|
||||
sus1VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus2valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus2orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus2Value, susParameters->sus2coeffAlpha,
|
||||
susParameters->sus2coeffBeta),
|
||||
sus2VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus3valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus3orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus3Value, susParameters->sus3coeffAlpha,
|
||||
susParameters->sus3coeffBeta),
|
||||
sus3VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus4valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus4orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus4Value, susParameters->sus4coeffAlpha,
|
||||
susParameters->sus4coeffBeta),
|
||||
sus4VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus5valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus5orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus5Value, susParameters->sus5coeffAlpha,
|
||||
susParameters->sus5coeffBeta),
|
||||
sus5VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus6valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus6orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus6Value, susParameters->sus6coeffAlpha,
|
||||
susParameters->sus6coeffBeta),
|
||||
sus6VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus7valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus7orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus7Value, susParameters->sus7coeffAlpha,
|
||||
susParameters->sus7coeffBeta),
|
||||
sus7VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus8valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus8orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus8Value, susParameters->sus8coeffAlpha,
|
||||
susParameters->sus8coeffBeta),
|
||||
sus8VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus9valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus9orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus9Value, susParameters->sus9coeffAlpha,
|
||||
susParameters->sus9coeffBeta),
|
||||
sus9VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus10valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus10orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus10Value, susParameters->sus10coeffAlpha,
|
||||
susParameters->sus10coeffBeta),
|
||||
sus10VecBody, 3, 3, 1);
|
||||
}
|
||||
if (sus11valid) {
|
||||
MatrixOperations<float>::multiply(
|
||||
susParameters->sus11orientationMatrix[0],
|
||||
susConverter.getSunVectorSensorFrame(sus11Value, susParameters->sus11coeffAlpha,
|
||||
susParameters->sus11coeffBeta),
|
||||
sus11VecBody, 3, 3, 1);
|
||||
}
|
||||
float susVecSensor[12][3] = {{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}};
|
||||
float susVecBody[12][3] = {{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}};
|
||||
|
||||
/* ------ Mean Value: susDirEst ------ */
|
||||
bool validIds[12] = {sus0valid, sus1valid, sus2valid, sus3valid, sus4valid, sus5valid,
|
||||
sus6valid, sus7valid, sus8valid, sus9valid, sus10valid, sus11valid};
|
||||
float susVecBody[3][12] = {{sus0VecBody[0], sus1VecBody[0], sus2VecBody[0], sus3VecBody[0],
|
||||
sus4VecBody[0], sus5VecBody[0], sus6VecBody[0], sus7VecBody[0],
|
||||
sus8VecBody[0], sus9VecBody[0], sus10VecBody[0], sus11VecBody[0]},
|
||||
{sus0VecBody[1], sus1VecBody[1], sus2VecBody[1], sus3VecBody[1],
|
||||
sus4VecBody[1], sus5VecBody[1], sus6VecBody[1], sus7VecBody[1],
|
||||
sus8VecBody[1], sus9VecBody[1], sus10VecBody[1], sus11VecBody[1]},
|
||||
{sus0VecBody[2], sus1VecBody[2], sus2VecBody[2], sus3VecBody[2],
|
||||
sus4VecBody[2], sus5VecBody[2], sus6VecBody[2], sus7VecBody[2],
|
||||
sus8VecBody[2], sus9VecBody[2], sus10VecBody[2], sus11VecBody[2]}};
|
||||
if (susValid[0]) {
|
||||
susConverter.calculateSunVector(susVecSensor[0], sus0Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus0orientationMatrix[0], susVecSensor[0],
|
||||
susVecBody[0], 3, 3, 1);
|
||||
}
|
||||
if (susValid[1]) {
|
||||
susConverter.calculateSunVector(susVecSensor[1], sus1Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus1orientationMatrix[0], susVecSensor[1],
|
||||
susVecBody[1], 3, 3, 1);
|
||||
}
|
||||
if (susValid[2]) {
|
||||
susConverter.calculateSunVector(susVecSensor[2], sus2Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus2orientationMatrix[0], susVecSensor[2],
|
||||
susVecBody[2], 3, 3, 1);
|
||||
}
|
||||
if (susValid[3]) {
|
||||
susConverter.calculateSunVector(susVecSensor[3], sus3Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus3orientationMatrix[0], susVecSensor[3],
|
||||
susVecBody[3], 3, 3, 1);
|
||||
}
|
||||
if (susValid[4]) {
|
||||
susConverter.calculateSunVector(susVecSensor[4], sus4Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus4orientationMatrix[0], susVecSensor[4],
|
||||
susVecBody[4], 3, 3, 1);
|
||||
}
|
||||
if (susValid[5]) {
|
||||
susConverter.calculateSunVector(susVecSensor[5], sus5Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus5orientationMatrix[0], susVecSensor[5],
|
||||
susVecBody[5], 3, 3, 1);
|
||||
}
|
||||
if (susValid[6]) {
|
||||
susConverter.calculateSunVector(susVecSensor[6], sus6Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus6orientationMatrix[0], susVecSensor[6],
|
||||
susVecBody[6], 3, 3, 1);
|
||||
}
|
||||
if (susValid[7]) {
|
||||
susConverter.calculateSunVector(susVecSensor[7], sus7Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus7orientationMatrix[0], susVecSensor[7],
|
||||
susVecBody[7], 3, 3, 1);
|
||||
}
|
||||
if (susValid[8]) {
|
||||
susConverter.calculateSunVector(susVecSensor[8], sus8Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus8orientationMatrix[0], susVecSensor[8],
|
||||
susVecBody[8], 3, 3, 1);
|
||||
}
|
||||
if (susValid[9]) {
|
||||
susConverter.calculateSunVector(susVecSensor[9], sus9Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus9orientationMatrix[0], susVecSensor[9],
|
||||
susVecBody[9], 3, 3, 1);
|
||||
}
|
||||
if (susValid[10]) {
|
||||
susConverter.calculateSunVector(susVecSensor[10], sus10Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus10orientationMatrix[0], susVecSensor[10],
|
||||
susVecBody[10], 3, 3, 1);
|
||||
}
|
||||
if (susValid[11]) {
|
||||
susConverter.calculateSunVector(susVecSensor[11], sus11Value);
|
||||
MatrixOperations<float>::multiply(susParameters->sus11orientationMatrix[0], susVecSensor[11],
|
||||
susVecBody[11], 3, 3, 1);
|
||||
}
|
||||
|
||||
double susMeanValue[3] = {0, 0, 0};
|
||||
for (uint8_t i = 0; i < 12; i++) {
|
||||
if (validIds[i]) {
|
||||
susMeanValue[0] += susVecBody[0][i];
|
||||
susMeanValue[1] += susVecBody[1][i];
|
||||
susMeanValue[2] += susVecBody[2][i];
|
||||
}
|
||||
susMeanValue[0] += susVecBody[i][0];
|
||||
susMeanValue[1] += susVecBody[i][1];
|
||||
susMeanValue[2] += susVecBody[i][2];
|
||||
}
|
||||
double susVecTot[3] = {0.0, 0.0, 0.0};
|
||||
VectorOperations<double>::normalize(susMeanValue, susVecTot, 3);
|
||||
@ -406,29 +367,29 @@ void SensorProcessing::processSus(
|
||||
{
|
||||
PoolReadGuard pg(susDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
std::memcpy(susDataProcessed->sus0vec.value, sus0VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus0vec.value, susVecBody[0], 3 * sizeof(float));
|
||||
susDataProcessed->sus0vec.setValid(sus0valid);
|
||||
std::memcpy(susDataProcessed->sus1vec.value, sus1VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus1vec.value, susVecBody[1], 3 * sizeof(float));
|
||||
susDataProcessed->sus1vec.setValid(sus1valid);
|
||||
std::memcpy(susDataProcessed->sus2vec.value, sus2VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus2vec.value, susVecBody[2], 3 * sizeof(float));
|
||||
susDataProcessed->sus2vec.setValid(sus2valid);
|
||||
std::memcpy(susDataProcessed->sus3vec.value, sus3VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus3vec.value, susVecBody[3], 3 * sizeof(float));
|
||||
susDataProcessed->sus3vec.setValid(sus3valid);
|
||||
std::memcpy(susDataProcessed->sus4vec.value, sus4VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus4vec.value, susVecBody[4], 3 * sizeof(float));
|
||||
susDataProcessed->sus4vec.setValid(sus4valid);
|
||||
std::memcpy(susDataProcessed->sus5vec.value, sus5VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus5vec.value, susVecBody[5], 3 * sizeof(float));
|
||||
susDataProcessed->sus5vec.setValid(sus5valid);
|
||||
std::memcpy(susDataProcessed->sus6vec.value, sus6VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus6vec.value, susVecBody[6], 3 * sizeof(float));
|
||||
susDataProcessed->sus6vec.setValid(sus6valid);
|
||||
std::memcpy(susDataProcessed->sus7vec.value, sus7VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus7vec.value, susVecBody[7], 3 * sizeof(float));
|
||||
susDataProcessed->sus7vec.setValid(sus7valid);
|
||||
std::memcpy(susDataProcessed->sus8vec.value, sus8VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus8vec.value, susVecBody[8], 3 * sizeof(float));
|
||||
susDataProcessed->sus8vec.setValid(sus8valid);
|
||||
std::memcpy(susDataProcessed->sus9vec.value, sus9VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus9vec.value, susVecBody[9], 3 * sizeof(float));
|
||||
susDataProcessed->sus9vec.setValid(sus9valid);
|
||||
std::memcpy(susDataProcessed->sus10vec.value, sus10VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus10vec.value, susVecBody[10], 3 * sizeof(float));
|
||||
susDataProcessed->sus10vec.setValid(sus10valid);
|
||||
std::memcpy(susDataProcessed->sus11vec.value, sus11VecBody, 3 * sizeof(float));
|
||||
std::memcpy(susDataProcessed->sus11vec.value, susVecBody[11], 3 * sizeof(float));
|
||||
susDataProcessed->sus11vec.setValid(sus11valid);
|
||||
std::memcpy(susDataProcessed->susVecTot.value, susVecTot, 3 * sizeof(double));
|
||||
susDataProcessed->susVecTot.setValid(true);
|
||||
@ -459,12 +420,11 @@ void SensorProcessing::processGyr(
|
||||
{
|
||||
PoolReadGuard pg(gyrDataProcessed);
|
||||
if (pg.getReadResult() == returnvalue::OK) {
|
||||
double zeroVector[3] = {0.0, 0.0, 0.0};
|
||||
std::memcpy(gyrDataProcessed->gyr0vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr1vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr2vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr3vec.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyrVecTot.value, zeroVector, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr0vec.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr1vec.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr2vec.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyr3vec.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
std::memcpy(gyrDataProcessed->gyrVecTot.value, ZERO_VEC_D, 3 * sizeof(double));
|
||||
gyrDataProcessed->setValidity(false, true);
|
||||
}
|
||||
}
|
||||
|
||||
@ -23,6 +23,9 @@ class SensorProcessing {
|
||||
acsctrl::GpsDataProcessed *gpsDataProcessed,
|
||||
const AcsParameters *acsParameters); // Will call protected functions
|
||||
private:
|
||||
static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
|
||||
static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
|
||||
|
||||
protected:
|
||||
// short description needed for every function
|
||||
void processMgm(const float *mgm0Value, bool mgm0valid, const float *mgm1Value, bool mgm1valid,
|
||||
|
||||
@ -1,121 +1,64 @@
|
||||
#include "SusConverter.h"
|
||||
|
||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||
#include <fsfw/datapoollocal/LocalPoolVector.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
bool SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
|
||||
if (susChannel[0] <= susChannelValueCheckLow || susChannel[0] > susChannelValueCheckHigh ||
|
||||
uint64_t SusConverter::checkSunSensorData(const uint16_t susChannel[6]) {
|
||||
if (susChannel[0] <= SUS_CHANNEL_VALUE_LOW || susChannel[0] > SUS_CHANNEL_VALUE_HIGH ||
|
||||
susChannel[0] > susChannel[GNDREF]) {
|
||||
return false;
|
||||
return 0;
|
||||
}
|
||||
if (susChannel[1] <= susChannelValueCheckLow || susChannel[1] > susChannelValueCheckHigh ||
|
||||
if (susChannel[1] <= SUS_CHANNEL_VALUE_LOW || susChannel[1] > SUS_CHANNEL_VALUE_HIGH ||
|
||||
susChannel[1] > susChannel[GNDREF]) {
|
||||
return false;
|
||||
return 0;
|
||||
};
|
||||
if (susChannel[2] <= susChannelValueCheckLow || susChannel[2] > susChannelValueCheckHigh ||
|
||||
if (susChannel[2] <= SUS_CHANNEL_VALUE_LOW || susChannel[2] > SUS_CHANNEL_VALUE_HIGH ||
|
||||
susChannel[2] > susChannel[GNDREF]) {
|
||||
return false;
|
||||
return 0;
|
||||
};
|
||||
if (susChannel[3] <= susChannelValueCheckLow || susChannel[3] > susChannelValueCheckHigh ||
|
||||
if (susChannel[3] <= SUS_CHANNEL_VALUE_LOW || susChannel[3] > SUS_CHANNEL_VALUE_HIGH ||
|
||||
susChannel[3] > susChannel[GNDREF]) {
|
||||
return false;
|
||||
return 0;
|
||||
};
|
||||
|
||||
susChannelValueSum =
|
||||
uint64_t susChannelValueSum =
|
||||
4 * susChannel[GNDREF] - (susChannel[0] + susChannel[1] + susChannel[2] + susChannel[3]);
|
||||
if ((susChannelValueSum < susChannelValueSumHigh) &&
|
||||
(susChannelValueSum > susChannelValueSumLow)) {
|
||||
return false;
|
||||
if (susChannelValueSum < SUS_ALBEDO_CHECK) {
|
||||
return 0;
|
||||
};
|
||||
return true;
|
||||
return susChannelValueSum;
|
||||
}
|
||||
|
||||
void SusConverter::calcAngle(const uint16_t susChannel[6]) {
|
||||
float xout, yout;
|
||||
float s = 0.03; // s=[mm] gap between diodes
|
||||
uint8_t d = 5; // d=[mm] edge length of the quadratic aperture
|
||||
uint8_t h = 1; // h=[mm] distance between diodes and aperture
|
||||
int ch0, ch1, ch2, ch3;
|
||||
bool SusConverter::checkValidity(bool* susValid, const uint64_t brightness[12],
|
||||
const float threshold) {
|
||||
uint8_t maxBrightness = 0;
|
||||
VectorOperations<uint64_t>::maxValue(brightness, 12, &maxBrightness);
|
||||
if (brightness[maxBrightness] == 0) {
|
||||
return true;
|
||||
}
|
||||
for (uint8_t idx = 0; idx < 12; idx++) {
|
||||
if ((idx != maxBrightness) and (brightness[idx] < threshold * brightness[maxBrightness])) {
|
||||
susValid[idx] = false;
|
||||
continue;
|
||||
}
|
||||
susValid[idx] = true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void SusConverter::calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]) {
|
||||
// Substract measurement values from GNDREF zero current threshold
|
||||
ch0 = susChannel[GNDREF] - susChannel[0];
|
||||
ch1 = susChannel[GNDREF] - susChannel[1];
|
||||
ch2 = susChannel[GNDREF] - susChannel[2];
|
||||
ch3 = susChannel[GNDREF] - susChannel[3];
|
||||
float ch0 = susChannel[GNDREF] - susChannel[0];
|
||||
float ch1 = susChannel[GNDREF] - susChannel[1];
|
||||
float ch2 = susChannel[GNDREF] - susChannel[2];
|
||||
float ch3 = susChannel[GNDREF] - susChannel[3];
|
||||
|
||||
// Calculation of x and y
|
||||
xout = ((d - s) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm]
|
||||
yout = ((d - s) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm]
|
||||
float xout = ((D - S) / 2) * (ch2 - ch3 - ch0 + ch1) / (ch0 + ch1 + ch2 + ch3); //[mm]
|
||||
float yout = ((D - S) / 2) * (ch2 + ch3 - ch0 - ch1) / (ch0 + ch1 + ch2 + ch3); //[mm]
|
||||
|
||||
// Calculation of the angles
|
||||
alphaBetaRaw[0] = atan2(xout, h) * (180 / M_PI); //[°]
|
||||
alphaBetaRaw[1] = atan2(yout, h) * (180 / M_PI); //[°]
|
||||
}
|
||||
|
||||
void SusConverter::calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]) {
|
||||
uint8_t index, k, l;
|
||||
|
||||
// while loop iterates above all calibration cells to use the different calibration functions in
|
||||
// each cell
|
||||
k = 0;
|
||||
while (k < 3) {
|
||||
k++;
|
||||
l = 0;
|
||||
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))) {
|
||||
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] +
|
||||
coeffAlpha[index][2] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
|
||||
coeffAlpha[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
|
||||
coeffAlpha[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
|
||||
coeffAlpha[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°]
|
||||
alphaBetaCalibrated[1] =
|
||||
coeffBeta[index][0] + coeffBeta[index][1] * alphaBetaRaw[0] +
|
||||
coeffBeta[index][2] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][3] * alphaBetaRaw[0] * alphaBetaRaw[0] +
|
||||
coeffBeta[index][4] * alphaBetaRaw[0] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][5] * alphaBetaRaw[1] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][6] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[0] +
|
||||
coeffBeta[index][7] * alphaBetaRaw[0] * alphaBetaRaw[0] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][8] * alphaBetaRaw[0] * alphaBetaRaw[1] * alphaBetaRaw[1] +
|
||||
coeffBeta[index][9] * alphaBetaRaw[1] * alphaBetaRaw[1] * alphaBetaRaw[1]; //[°]
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float* SusConverter::calculateSunVector() {
|
||||
// Calculate the normalized Sun Vector
|
||||
sunVectorSensorFrame[0] = -(tan(alphaBetaCalibrated[0] * (M_PI / 180)) /
|
||||
(sqrt((powf(tan(alphaBetaCalibrated[0] * (M_PI / 180)), 2)) +
|
||||
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
|
||||
sunVectorSensorFrame[1] = -(tan(alphaBetaCalibrated[1] * (M_PI / 180)) /
|
||||
(sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
|
||||
powf(tan((alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
|
||||
sunVectorSensorFrame[2] =
|
||||
-(-1 / (sqrt(powf((tan(alphaBetaCalibrated[0] * (M_PI / 180))), 2) +
|
||||
powf((tan(alphaBetaCalibrated[1] * (M_PI / 180))), 2) + (1))));
|
||||
|
||||
return sunVectorSensorFrame;
|
||||
}
|
||||
|
||||
float* SusConverter::getSunVectorSensorFrame(const uint16_t susChannel[6],
|
||||
const float coeffAlpha[9][10],
|
||||
const float coeffBeta[9][10]) {
|
||||
calcAngle(susChannel);
|
||||
calibration(coeffAlpha, coeffBeta);
|
||||
return calculateSunVector();
|
||||
sunVectorSensorFrame[0] = -xout;
|
||||
sunVectorSensorFrame[1] = -yout;
|
||||
sunVectorSensorFrame[2] = H;
|
||||
VectorOperations<float>::normalize(sunVectorSensorFrame, sunVectorSensorFrame, 3);
|
||||
}
|
||||
|
||||
@ -1,8 +1,4 @@
|
||||
#ifndef MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
|
||||
#define MISSION_CONTROLLER_ACS_SUSCONVERTER_H_
|
||||
|
||||
#include <fsfw/datapoollocal/LocalPoolVector.h>
|
||||
#include <stdint.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
|
||||
#include "AcsParameters.h"
|
||||
|
||||
@ -10,41 +6,26 @@ class SusConverter {
|
||||
public:
|
||||
SusConverter() {}
|
||||
|
||||
bool checkSunSensorData(const uint16_t susChannel[6]);
|
||||
|
||||
void calcAngle(const uint16_t susChannel[6]);
|
||||
void calibration(const float coeffAlpha[9][10], const float coeffBeta[9][10]);
|
||||
float* calculateSunVector();
|
||||
|
||||
float* getSunVectorSensorFrame(const uint16_t susChannel[6], const float coeffAlpha[9][10],
|
||||
const float coeffBeta[9][10]);
|
||||
uint64_t checkSunSensorData(const uint16_t susChannel[6]);
|
||||
bool checkValidity(bool* susValid, const uint64_t brightness[12], const float threshold);
|
||||
void calculateSunVector(float* sunVectorSensorFrame, const uint16_t susChannel[6]);
|
||||
|
||||
private:
|
||||
float alphaBetaRaw[2]; //[°]
|
||||
float alphaBetaCalibrated[2]; //[°]
|
||||
float sunVectorSensorFrame[3]; //[-]
|
||||
|
||||
bool validFlag[12] = {returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,
|
||||
returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK,
|
||||
returnvalue::OK, returnvalue::OK, returnvalue::OK, returnvalue::OK};
|
||||
|
||||
static const uint8_t GNDREF = 4;
|
||||
uint16_t susChannelValueCheckHigh =
|
||||
4096; //=2^12[Bit]high borderline for the channel values of one sun sensor for validity Check
|
||||
uint8_t susChannelValueCheckLow =
|
||||
0; //[Bit]low borderline for the channel values of one sun sensor for validity Check
|
||||
uint16_t susChannelValueSumHigh =
|
||||
100; // 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by
|
||||
// the reflection of sunlight from the moon/earth
|
||||
uint8_t susChannelValueSumLow =
|
||||
0; //[Bit]low borderline for check if the sun sensor is illuminated
|
||||
// by the sun or by the reflection of sunlight from the moon/earth
|
||||
uint8_t completeCellWidth = 140,
|
||||
halfCellWidth = 70; //[°] Width of the calibration cells --> necessary for checking in
|
||||
// which cell a data point should be
|
||||
uint16_t susChannelValueSum = 0;
|
||||
// =2^12[Bit]high borderline for the channel values of one sun sensor for validity Check
|
||||
static constexpr uint16_t SUS_CHANNEL_VALUE_HIGH = 4096;
|
||||
// [Bit]low borderline for the channel values of one sun sensor for validity Check
|
||||
static constexpr uint8_t SUS_CHANNEL_VALUE_LOW = 0;
|
||||
// 4096[Bit]high borderline for check if the sun sensor is illuminated by the sun or by the
|
||||
// reflection of sunlight from the moon/earth
|
||||
static constexpr uint16_t SUS_ALBEDO_CHECK = 1000;
|
||||
// [Bit]low borderline for check if the sun sensor is illuminated by the sun or by the reflection
|
||||
// of sunlight from the moon/earth
|
||||
static constexpr uint8_t SUS_CHANNEL_SUM_LOW = 0;
|
||||
|
||||
static constexpr float S = 0.03; // S=[mm] gap between diodes
|
||||
static constexpr float D = 5; // D=[mm] edge length of the quadratic aperture
|
||||
static constexpr float H = 2.5; // H=[mm] distance between diodes and aperture
|
||||
|
||||
AcsParameters acsParameters;
|
||||
};
|
||||
|
||||
#endif /* MISSION_CONTROLLER_ACS_SUSCONVERTER_H_ */
|
||||
|
||||
@ -7,8 +7,10 @@ Detumble::Detumble() {}
|
||||
|
||||
Detumble::~Detumble() {}
|
||||
|
||||
uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid, const bool useFullDetumbleLaw) {
|
||||
acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid,
|
||||
const bool satRotRateValid,
|
||||
const bool magFieldRateValid,
|
||||
const bool useFullDetumbleLaw) {
|
||||
if (not magFieldValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
} else if (satRotRateValid and useFullDetumbleLaw) {
|
||||
|
||||
@ -11,8 +11,9 @@ class Detumble {
|
||||
Detumble();
|
||||
virtual ~Detumble();
|
||||
|
||||
uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid, const bool useFullDetumbleLaw);
|
||||
acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid,
|
||||
const bool magFieldRateValid,
|
||||
const bool useFullDetumbleLaw);
|
||||
|
||||
void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB,
|
||||
double gain);
|
||||
|
||||
@ -5,9 +5,6 @@
|
||||
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
|
||||
#include <fsfw/globalfunctions/math/VectorOperations.h>
|
||||
#include <fsfw/globalfunctions/sign.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "../util/MathOperations.h"
|
||||
|
||||
PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; }
|
||||
|
||||
@ -32,12 +29,13 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
||||
double qErrorLaw[3] = {0, 0, 0};
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (abs(qError[i]) < qErrorMin) {
|
||||
if (std::abs(qError[i]) < qErrorMin) {
|
||||
qErrorLaw[i] = qErrorMin;
|
||||
} else {
|
||||
qErrorLaw[i] = abs(qError[i]);
|
||||
qErrorLaw[i] = std::abs(qError[i]);
|
||||
}
|
||||
}
|
||||
|
||||
double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);
|
||||
|
||||
double gain1 = cInt * omMax / qErrorLawNorm;
|
||||
@ -73,7 +71,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
||||
double pErrorSign[3] = {0, 0, 0};
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (abs(pError[i]) > 1) {
|
||||
if (std::abs(pError[i]) > 1) {
|
||||
pErrorSign[i] = sign(pError[i]);
|
||||
} else {
|
||||
pErrorSign[i] = pError[i];
|
||||
@ -98,61 +96,92 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters
|
||||
VectorOperations<double>::mulScalar(torqueRws, -1, torqueRws, 4);
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
|
||||
const int32_t speedRw3, double *rwTrqNs) {
|
||||
// concentrate RW speeds as vector and convert to double
|
||||
double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
|
||||
static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
|
||||
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
|
||||
VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
|
||||
|
||||
// calculate RPM offset utilizing the nullspace
|
||||
double rpmOffset[4] = {0, 0, 0, 0};
|
||||
double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC;
|
||||
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed,
|
||||
rpmOffset, 4);
|
||||
|
||||
// calculate resulting angular momentum
|
||||
double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
|
||||
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
|
||||
rwAngMomentum, 4);
|
||||
|
||||
// calculate resulting torque
|
||||
double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MatrixOperations<double>::multiply(acsParameters->rwMatrices.nullspaceVector,
|
||||
acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4,
|
||||
1, 4);
|
||||
MatrixOperations<double>::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs,
|
||||
4);
|
||||
}
|
||||
|
||||
void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
double *magFieldEst, bool magFieldEstValid, double *satRate,
|
||||
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
|
||||
int32_t *speedRw3, double *mgtDpDes) {
|
||||
if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) {
|
||||
mgtDpDes[0] = 0;
|
||||
mgtDpDes[1] = 0;
|
||||
mgtDpDes[2] = 0;
|
||||
const double *magFieldB, const bool magFieldBValid,
|
||||
const double *satRate, const int32_t speedRw0, const int32_t speedRw1,
|
||||
const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) {
|
||||
if (not magFieldBValid or not pointingLawParameters->desatOn) {
|
||||
return;
|
||||
}
|
||||
|
||||
// calculating momentum of satellite and momentum of reaction wheels
|
||||
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
|
||||
double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
|
||||
momentumRwU, 4);
|
||||
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU,
|
||||
momentumRw, 3, 4, 1);
|
||||
double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
|
||||
momentumSat, 3, 3, 1);
|
||||
VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
|
||||
// calculating momentum error
|
||||
double deltaMomentum[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(momentumTotal, pointingLawParameters->desatMomentumRef,
|
||||
deltaMomentum, 3);
|
||||
// resulting magnetic dipole command
|
||||
double crossMomentumMagField[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
|
||||
double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
|
||||
factor = (pointingLawParameters->deSatGainFactor) / normMag;
|
||||
VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
|
||||
}
|
||||
// concentrate RW speeds as vector and convert to double
|
||||
double speedRws[4] = {static_cast<double>(speedRw0), static_cast<double>(speedRw1),
|
||||
static_cast<double>(speedRw2), static_cast<double>(speedRw3)};
|
||||
|
||||
void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const int32_t *speedRw0, const int32_t *speedRw1,
|
||||
const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
|
||||
double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3};
|
||||
double wheelMomentum[4] = {0, 0, 0, 0};
|
||||
double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
|
||||
// conversion to [rad/s] for further calculations
|
||||
VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
|
||||
VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
|
||||
double diffRwSpeed[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
|
||||
VectorOperations<double>::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel,
|
||||
wheelMomentum, 4);
|
||||
double gainNs = pointingLawParameters->gainNullspace;
|
||||
double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
||||
MathOperations<double>::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace,
|
||||
acsParameters->rwMatrices.nullspace,
|
||||
*nullSpaceMatrix, 4);
|
||||
MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
|
||||
VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
|
||||
// convert magFieldB from uT to T
|
||||
double magFieldBT[3] = {0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(magFieldB, 1e-6, magFieldBT, 3);
|
||||
|
||||
// calculate angular momentum of the satellite
|
||||
double angMomentumSat[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate,
|
||||
angMomentumSat, 3, 3, 1);
|
||||
|
||||
// calculate angular momentum of the reaction wheels with respect to the nullspace RW speed
|
||||
// relocate RW speed zero to nullspace RW speed
|
||||
double refSpeedRws[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(acsParameters->rwMatrices.nullspaceVector,
|
||||
pointingLawParameters->nullspaceSpeed, refSpeedRws, 4);
|
||||
VectorOperations<double>::subtract(speedRws, refSpeedRws, speedRws, 4);
|
||||
// convert speed from 10 RPM to 1 RPM
|
||||
VectorOperations<double>::mulScalar(speedRws, 1e-1, speedRws, 4);
|
||||
// convert to rad/s
|
||||
VectorOperations<double>::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4);
|
||||
// calculate angular momentum of each RW
|
||||
double angMomentumRwU[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel,
|
||||
angMomentumRwU, 4);
|
||||
// convert RW angular momentum to body RF
|
||||
double angMomentumRw[3] = {0, 0, 0};
|
||||
MatrixOperations<double>::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU,
|
||||
angMomentumRw, 3, 4, 1);
|
||||
|
||||
// calculate total angular momentum
|
||||
double angMomentumTotal[3] = {0, 0, 0};
|
||||
VectorOperations<double>::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3);
|
||||
|
||||
// calculating momentum error
|
||||
double deltaAngMomentum[3] = {0, 0, 0};
|
||||
VectorOperations<double>::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef,
|
||||
deltaAngMomentum, 3);
|
||||
|
||||
// resulting magnetic dipole command
|
||||
double crossAngMomentumMagField[3] = {0, 0, 0};
|
||||
VectorOperations<double>::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField);
|
||||
double factor =
|
||||
pointingLawParameters->deSatGainFactor / VectorOperations<double>::norm(magFieldBT, 3);
|
||||
VectorOperations<double>::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3);
|
||||
}
|
||||
|
||||
void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) {
|
||||
@ -169,15 +198,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
|
||||
if (rwCmdSpeeds[i] != 0) {
|
||||
if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed &&
|
||||
rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) {
|
||||
if (currRwSpeed[i] == 0) {
|
||||
if (rwCmdSpeeds[i] > 0) {
|
||||
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
|
||||
} else if (rwCmdSpeeds[i] < 0) {
|
||||
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
|
||||
}
|
||||
} else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) {
|
||||
if (rwCmdSpeeds[i] > currRwSpeed[i]) {
|
||||
rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed;
|
||||
} else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) {
|
||||
} else if (rwCmdSpeeds[i] < currRwSpeed[i]) {
|
||||
rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,13 +1,10 @@
|
||||
#ifndef PTGCTRL_H_
|
||||
#define PTGCTRL_H_
|
||||
|
||||
#include <math.h>
|
||||
#include <mission/controller/acs/AcsParameters.h>
|
||||
#include <mission/controller/acs/SensorValues.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
|
||||
#include "../AcsParameters.h"
|
||||
#include "../SensorValues.h"
|
||||
#include "eive/resultClassIds.h"
|
||||
|
||||
class PtgCtrl {
|
||||
/*
|
||||
@ -29,14 +26,14 @@ class PtgCtrl {
|
||||
void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError,
|
||||
const double *deltaRate, const double *rwPseudoInv, double *torqueRws);
|
||||
|
||||
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
double *magFieldEst, bool magFieldEstValid, double *satRate,
|
||||
int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3,
|
||||
double *mgtDpDes);
|
||||
|
||||
void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2,
|
||||
const int32_t *speedRw3, double *rwTrqNs);
|
||||
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
|
||||
const int32_t speedRw3, double *rwTrqNs);
|
||||
|
||||
void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters,
|
||||
const double *magFieldB, const bool magFieldBValid, const double *satRate,
|
||||
const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2,
|
||||
const int32_t speedRw3, double *mgtDpDes);
|
||||
|
||||
/* @brief: Commands the stiction torque in case wheel speed is to low
|
||||
* torqueCommand modified torque after antistiction
|
||||
@ -45,6 +42,7 @@ class PtgCtrl {
|
||||
|
||||
private:
|
||||
const AcsParameters *acsParameters;
|
||||
static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60;
|
||||
};
|
||||
|
||||
#endif /* ACS_CONTROL_PTGCTRL_H_ */
|
||||
|
||||
@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter
|
||||
|
||||
SafeCtrl::~SafeCtrl() {}
|
||||
|
||||
uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled, const uint8_t dampingEnabled) {
|
||||
acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled,
|
||||
const uint8_t dampingEnabled) {
|
||||
if (not magFieldValid) {
|
||||
return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL;
|
||||
} else if (mekfEnabled and mekfValid) {
|
||||
|
||||
@ -12,9 +12,9 @@ class SafeCtrl {
|
||||
SafeCtrl(AcsParameters *acsParameters_);
|
||||
virtual ~SafeCtrl();
|
||||
|
||||
uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled, const uint8_t dampingEnabled);
|
||||
acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid,
|
||||
const bool satRotRateValid, const bool sunDirValid,
|
||||
const uint8_t mekfEnabled, const uint8_t dampingEnabled);
|
||||
|
||||
void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI,
|
||||
const double *quatBI, const double *sunDirRefB, double *magMomB,
|
||||
|
||||
@ -9,6 +9,85 @@
|
||||
|
||||
namespace tcsCtrl {
|
||||
|
||||
/**
|
||||
* NOP Limit: Hard limit for device, usually from datasheet. Device damage is possible lif NOP limit
|
||||
* is exceeded.
|
||||
* OP Limit: Soft limit. Device should be switched off or TCS controller should take action if the
|
||||
* limit is exceeded to avoid reaching NOP limit
|
||||
*/
|
||||
struct TempLimits {
|
||||
TempLimits(float nopLowerLimit, float opLowerLimit, float cutOffLimit, float opUpperLimit,
|
||||
float nopUpperLimit)
|
||||
: opLowerLimit(opLowerLimit),
|
||||
opUpperLimit(opUpperLimit),
|
||||
cutOffLimit(cutOffLimit),
|
||||
nopLowerLimit(nopLowerLimit),
|
||||
nopUpperLimit(nopUpperLimit) {}
|
||||
float opLowerLimit;
|
||||
float opUpperLimit;
|
||||
float cutOffLimit;
|
||||
float nopLowerLimit;
|
||||
float nopUpperLimit;
|
||||
};
|
||||
|
||||
/**
|
||||
* Abstraction for the state of a single thermal component
|
||||
*/
|
||||
struct ThermalState {
|
||||
uint8_t noSensorAvailableCounter;
|
||||
// Which sensor is used for this component?
|
||||
uint8_t sensorIndex = 0;
|
||||
// Is heating on for that thermal module?
|
||||
bool heating = false;
|
||||
// Which switch is being used for heating the component
|
||||
heater::Switch heaterSwitch = heater::Switch::HEATER_NONE;
|
||||
// Heater start time and end times as UNIX seconds. Please note that these times will be updated
|
||||
// when a switch command is sent, with no guarantess that the heater actually went on.
|
||||
uint32_t heaterStartTime = 0;
|
||||
uint32_t heaterEndTime = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* Abstraction for the state of a single heater.
|
||||
*/
|
||||
struct HeaterState {
|
||||
bool switchTransition = false;
|
||||
heater::SwitchState target = heater::SwitchState::OFF;
|
||||
uint8_t heaterSwitchControlCycles = 0;
|
||||
bool trackHeaterMaxBurnTime = false;
|
||||
Countdown heaterOnMaxBurnTime;
|
||||
};
|
||||
|
||||
using HeaterSwitchStates = std::array<heater::SwitchState, heater::NUMBER_OF_SWITCHES>;
|
||||
|
||||
enum ThermalComponents : uint8_t {
|
||||
NONE = 0,
|
||||
ACS_BOARD = 1,
|
||||
MGT = 2,
|
||||
RW = 3,
|
||||
STR = 4,
|
||||
IF_BOARD = 5,
|
||||
TCS_BOARD = 6,
|
||||
OBC = 7,
|
||||
LEGACY_OBCIF_BOARD = 8,
|
||||
SBAND_TRANSCEIVER = 9,
|
||||
PCDUP60_BOARD = 10,
|
||||
PCDUACU = 11,
|
||||
PCDUPDU = 12,
|
||||
PLPCDU_BOARD = 13,
|
||||
PLOCMISSION_BOARD = 14,
|
||||
PLOCPROCESSING_BOARD = 15,
|
||||
DAC = 16,
|
||||
CAMERA = 17,
|
||||
DRO = 18,
|
||||
X8 = 19,
|
||||
HPA = 20,
|
||||
TX = 21,
|
||||
MPA = 22,
|
||||
SCEX_BOARD = 23,
|
||||
NUM_THERMAL_COMPONENTS
|
||||
};
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::TCS_CONTROLLER;
|
||||
static constexpr Event NO_VALID_SENSOR_TEMPERATURE = MAKE_EVENT(0, severity::MEDIUM);
|
||||
static constexpr Event NO_HEALTHY_HEATER_AVAILABLE = MAKE_EVENT(1, severity::MEDIUM);
|
||||
@ -18,6 +97,12 @@ static constexpr Event CAMERA_OVERHEATING = MAKE_EVENT(5, severity::HIGH);
|
||||
static constexpr Event PCDU_SYSTEM_OVERHEATING = MAKE_EVENT(6, severity::HIGH);
|
||||
static constexpr Event HEATER_NOT_OFF_FOR_OFF_MODE = MAKE_EVENT(7, severity::MEDIUM);
|
||||
static constexpr Event MGT_OVERHEATING = MAKE_EVENT(8, severity::HIGH);
|
||||
//! [EXPORT] : [COMMENT] P1: Module index. P2: Heater index
|
||||
static constexpr Event TCS_SWITCHING_HEATER_ON = MAKE_EVENT(9, severity::INFO);
|
||||
//! [EXPORT] : [COMMENT] P1: Module index. P2: Heater index
|
||||
static constexpr Event TCS_SWITCHING_HEATER_OFF = MAKE_EVENT(10, severity::INFO);
|
||||
//! [EXPORT] : [COMMENT] P1: Heater index. P2: Maximum burn time for heater.
|
||||
static constexpr Event TCS_HEATER_MAX_BURN_TIME_REACHED = MAKE_EVENT(11, severity::MEDIUM);
|
||||
|
||||
enum SetId : uint32_t {
|
||||
SENSOR_TEMPERATURES = 0,
|
||||
@ -25,6 +110,7 @@ enum SetId : uint32_t {
|
||||
SUS_TEMPERATURES = 2,
|
||||
COMPONENT_TEMPERATURES = 3,
|
||||
HEATER_SET = 4,
|
||||
TCS_CTRL_INFO = 5
|
||||
};
|
||||
|
||||
enum PoolIds : lp_id_t {
|
||||
@ -92,7 +178,13 @@ enum PoolIds : lp_id_t {
|
||||
TEMP_ADC_PAYLOAD_PCDU,
|
||||
|
||||
HEATER_SWITCH_LIST,
|
||||
HEATER_CURRENT
|
||||
HEATER_CURRENT,
|
||||
|
||||
HEATER_ON_FOR_COMPONENT_VEC,
|
||||
SENSOR_USED_FOR_TCS_CTRL,
|
||||
HEATER_IDX_USED_FOR_TCS_CTRL,
|
||||
HEATER_START_TIME,
|
||||
HEATER_END_TIME
|
||||
};
|
||||
|
||||
static const uint8_t ENTRIES_SENSOR_TEMPERATURE_SET = 25;
|
||||
@ -232,6 +324,29 @@ class HeaterInfo : public StaticLocalDataSet<3> {
|
||||
lp_var_t<int16_t> heaterCurrent = lp_var_t<int16_t>(sid.objectId, PoolIds::HEATER_CURRENT, this);
|
||||
};
|
||||
|
||||
class TcsCtrlInfo : public StaticLocalDataSet<6> {
|
||||
public:
|
||||
explicit TcsCtrlInfo(HasLocalDataPoolIF* owner) : StaticLocalDataSet(owner, TCS_CTRL_INFO) {}
|
||||
|
||||
explicit TcsCtrlInfo(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, TCS_CTRL_INFO)) {}
|
||||
|
||||
lp_vec_t<uint8_t, tcsCtrl::NUM_THERMAL_COMPONENTS> heatingOnVec =
|
||||
lp_vec_t<uint8_t, tcsCtrl::NUM_THERMAL_COMPONENTS>(
|
||||
sid.objectId, PoolIds::HEATER_ON_FOR_COMPONENT_VEC, this);
|
||||
lp_vec_t<uint8_t, tcsCtrl::NUM_THERMAL_COMPONENTS> sensorIdxUsedForTcsCtrl =
|
||||
lp_vec_t<uint8_t, tcsCtrl::NUM_THERMAL_COMPONENTS>(sid.objectId,
|
||||
PoolIds::SENSOR_USED_FOR_TCS_CTRL, this);
|
||||
lp_vec_t<uint8_t, tcsCtrl::NUM_THERMAL_COMPONENTS> heaterSwitchIdx =
|
||||
lp_vec_t<uint8_t, tcsCtrl::NUM_THERMAL_COMPONENTS>(
|
||||
sid.objectId, PoolIds::HEATER_IDX_USED_FOR_TCS_CTRL, this);
|
||||
lp_vec_t<uint32_t, tcsCtrl::NUM_THERMAL_COMPONENTS> heaterStartTimes =
|
||||
lp_vec_t<uint32_t, tcsCtrl::NUM_THERMAL_COMPONENTS>(sid.objectId, PoolIds::HEATER_START_TIME,
|
||||
this);
|
||||
lp_vec_t<uint32_t, tcsCtrl::NUM_THERMAL_COMPONENTS> heaterEndTimes =
|
||||
lp_vec_t<uint32_t, tcsCtrl::NUM_THERMAL_COMPONENTS>(sid.objectId, PoolIds::HEATER_END_TIME,
|
||||
this);
|
||||
};
|
||||
|
||||
} // namespace tcsCtrl
|
||||
|
||||
#endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_THERMALCONTROLLERDEFINITIONS_H_ */
|
||||
|
||||
Reference in New Issue
Block a user