125 lines
4.1 KiB
C++
125 lines
4.1 KiB
C++
#include "fsfw/power/PowerSensor.h"
|
|
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
|
|
PowerSensor::PowerSensor(object_id_t objectId, sid_t setId, VariableIds ids, DefaultLimits limits,
|
|
SensorEvents events, uint16_t confirmationCount)
|
|
: SystemObject(objectId),
|
|
parameterHelper(this),
|
|
healthHelper(this, objectId),
|
|
powerSensorSet(setId),
|
|
current(ids.pidCurrent, &powerSensorSet),
|
|
voltage(ids.pidVoltage, &powerSensorSet),
|
|
power(ids.poolIdPower, &powerSensorSet, PoolVariableIF::VAR_WRITE),
|
|
currentLimit(objectId, MODULE_ID_CURRENT, ids.pidCurrent, confirmationCount,
|
|
limits.currentMin, limits.currentMax, events.currentLow, events.currentHigh),
|
|
voltageLimit(objectId, MODULE_ID_VOLTAGE, ids.pidVoltage, confirmationCount,
|
|
limits.voltageMin, limits.voltageMax, events.voltageLow, events.voltageHigh) {
|
|
commandQueue = QueueFactory::instance()->createMessageQueue();
|
|
}
|
|
|
|
PowerSensor::~PowerSensor() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
|
|
|
|
ReturnValue_t PowerSensor::calculatePower() {
|
|
powerSensorSet.read();
|
|
ReturnValue_t result1 = HasReturnvaluesIF::RETURN_FAILED;
|
|
ReturnValue_t result2 = HasReturnvaluesIF::RETURN_FAILED;
|
|
if (healthHelper.healthTable->isHealthy(getObjectId()) && voltage.isValid() &&
|
|
current.isValid()) {
|
|
result1 = voltageLimit.doCheck(voltage.value);
|
|
result2 = currentLimit.doCheck(current.value);
|
|
} else {
|
|
voltageLimit.setToInvalid();
|
|
currentLimit.setToInvalid();
|
|
result1 = OBJECT_NOT_HEALTHY;
|
|
}
|
|
if (result1 != HasReturnvaluesIF::RETURN_OK || result2 != HasReturnvaluesIF::RETURN_OK) {
|
|
result1 = MonitoringIF::INVALID;
|
|
power.setValid(PoolVariableIF::INVALID);
|
|
} else {
|
|
power.setValid(PoolVariableIF::VALID);
|
|
power.value = current.value * voltage.value;
|
|
}
|
|
powerSensorSet.commit();
|
|
return result1;
|
|
}
|
|
|
|
ReturnValue_t PowerSensor::performOperation(uint8_t opCode) {
|
|
checkCommandQueue();
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
MessageQueueId_t PowerSensor::getCommandQueue() const { return commandQueue->getId(); }
|
|
|
|
ReturnValue_t PowerSensor::initialize() {
|
|
ReturnValue_t result = SystemObject::initialize();
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = healthHelper.initialize();
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
result = parameterHelper.initialize();
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void PowerSensor::setAllMonitorsToUnchecked() {
|
|
currentLimit.setToUnchecked();
|
|
voltageLimit.setToUnchecked();
|
|
}
|
|
|
|
void PowerSensor::checkCommandQueue() {
|
|
CommandMessage command;
|
|
ReturnValue_t result = commandQueue->receiveMessage(&command);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
return;
|
|
}
|
|
result = healthHelper.handleHealthCommand(&command);
|
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
|
return;
|
|
}
|
|
result = parameterHelper.handleParameterMessage(&command);
|
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
|
return;
|
|
}
|
|
command.setToUnknownCommand();
|
|
commandQueue->reply(&command);
|
|
}
|
|
|
|
void PowerSensor::setDataPoolEntriesInvalid() {
|
|
powerSensorSet.read();
|
|
powerSensorSet.setValidity(false, true);
|
|
powerSensorSet.commit();
|
|
}
|
|
|
|
float PowerSensor::getPower() {
|
|
if (power.isValid()) {
|
|
return power.value;
|
|
} else {
|
|
return 0.0;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t PowerSensor::setHealth(HealthState health) {
|
|
healthHelper.setHealth(health);
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
HasHealthIF::HealthState PowerSensor::getHealth() { return healthHelper.getHealth(); }
|
|
|
|
ReturnValue_t PowerSensor::getParameter(uint8_t domainId, uint8_t uniqueId,
|
|
ParameterWrapper* parameterWrapper,
|
|
const ParameterWrapper* newValues, uint16_t startAtIndex) {
|
|
ReturnValue_t result =
|
|
currentLimit.getParameter(domainId, uniqueId, parameterWrapper, newValues, startAtIndex);
|
|
if (result != INVALID_DOMAIN_ID) {
|
|
return result;
|
|
}
|
|
result = voltageLimit.getParameter(domainId, uniqueId, parameterWrapper, newValues, startAtIndex);
|
|
return result;
|
|
}
|