eive-obsw/mission/devices/ACUHandler.cpp

213 lines
7.9 KiB
C++
Raw Normal View History

2021-01-28 14:55:21 +01:00
#include "ACUHandler.h"
2022-01-18 11:41:19 +01:00
2021-02-16 17:02:03 +01:00
#include "OBSWConfig.h"
2021-01-28 14:55:21 +01:00
2022-04-28 15:58:31 +02:00
ACUHandler::ACUHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie,
FailureIsolationBase *customFdir)
: GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir, ACU::MAX_CONFIGTABLE_ADDRESS,
2022-04-07 12:22:08 +02:00
ACU::MAX_HKTABLE_ADDRESS, ACU::HK_TABLE_REPLY_SIZE),
2022-05-23 11:25:58 +02:00
coreHk(this),
auxHk(this) {}
2021-01-28 14:55:21 +01:00
2022-01-18 11:41:19 +01:00
ACUHandler::~ACUHandler() {}
2021-01-28 14:55:21 +01:00
2022-01-18 11:41:19 +01:00
ReturnValue_t ACUHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = GOMSPACE::REQUEST_HK_TABLE;
return buildCommandFromCommand(*id, NULL, 0);
2021-02-12 14:31:43 +01:00
}
2022-05-23 11:25:58 +02:00
void ACUHandler::fillCommandAndReplyMap() { GomspaceDeviceHandler::fillCommandAndReplyMap(); }
2021-01-28 14:55:21 +01:00
void ACUHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {
2022-01-18 11:41:19 +01:00
parseHkTableReply(packet);
2022-04-26 10:37:25 +02:00
if (debugMode) {
#if OBSW_VERBOSE_LEVEL >= 1
2022-05-23 10:42:57 +02:00
PoolReadGuard pg0(&auxHk);
PoolReadGuard pg1(&coreHk);
2022-05-23 11:25:58 +02:00
if (pg0.getReadResult() != RETURN_OK or pg1.getReadResult() != RETURN_OK) {
2022-05-23 10:42:57 +02:00
return;
}
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 3; idx++) {
2022-05-23 10:42:57 +02:00
float tempC = coreHk.temperatures[idx] * 0.1;
sif::info << "ACU: Temperature " << idx << ": " << tempC << " °C" << std::endl;
}
2022-05-23 11:25:58 +02:00
sif::info << "ACU: Ground Watchdog Timer Count: " << auxHk.wdtCntGnd.value << std::endl;
2022-04-26 10:37:25 +02:00
sif::info << "ACU: Ground watchdog timer, seconds left before reboot: "
2022-05-23 10:42:57 +02:00
<< auxHk.wdtGndLeft.value << std::endl;
2021-02-14 11:40:40 +01:00
#endif
2022-04-26 10:37:25 +02:00
}
2021-01-28 14:55:21 +01:00
}
2021-02-12 14:31:43 +01:00
2022-04-07 12:22:08 +02:00
LocalPoolDataSetBase *ACUHandler::getDataSetHandle(sid_t sid) {
2022-05-23 10:42:57 +02:00
if (sid == coreHk.getSid()) {
return &coreHk;
2022-05-23 11:25:58 +02:00
} else if (sid == auxHk.getSid()) {
2022-05-23 10:42:57 +02:00
return &auxHk;
2022-04-07 12:22:08 +02:00
}
return nullptr;
}
2022-05-23 10:42:57 +02:00
ReturnValue_t ACUHandler::parseHkTableReply(const uint8_t *packet) {
2022-01-18 11:41:19 +01:00
uint16_t dataOffset = 0;
2022-05-23 10:42:57 +02:00
PoolReadGuard pg0(&coreHk);
PoolReadGuard pg1(&auxHk);
auto res0 = pg0.getReadResult();
auto res1 = pg1.getReadResult();
2022-05-23 11:25:58 +02:00
if (res0 != RETURN_OK) {
2022-05-23 10:42:57 +02:00
return res0;
}
2022-05-23 11:25:58 +02:00
if (res1 != RETURN_OK) {
2022-05-23 10:42:57 +02:00
return res1;
}
2022-01-18 11:41:19 +01:00
dataOffset += 12;
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 6; idx++) {
2022-05-23 10:42:57 +02:00
coreHk.currentInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
dataOffset += 4;
}
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 6; idx++) {
2022-05-23 10:42:57 +02:00
coreHk.voltageInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
dataOffset += 4;
}
2022-01-18 11:41:19 +01:00
2022-05-23 10:42:57 +02:00
coreHk.vcc = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
2022-01-18 11:41:19 +01:00
dataOffset += 4;
2022-05-23 10:42:57 +02:00
coreHk.vbat = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
2022-01-18 11:41:19 +01:00
dataOffset += 4;
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 3; idx++) {
2022-06-17 08:31:36 +02:00
coreHk.temperatures[idx] =
static_cast<int16_t>((packet[dataOffset] << 8) | packet[dataOffset + 1]) * 0.1;
2022-05-23 10:42:57 +02:00
dataOffset += 4;
}
2022-01-18 11:41:19 +01:00
2022-05-23 10:42:57 +02:00
coreHk.mpptMode = packet[dataOffset];
2022-01-18 11:41:19 +01:00
dataOffset += 3;
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 6; idx++) {
2022-05-23 10:42:57 +02:00
coreHk.vboostInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
dataOffset += 4;
}
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 6; idx++) {
2022-05-23 10:42:57 +02:00
coreHk.powerInChannels[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
dataOffset += 4;
}
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 3; idx++) {
2022-05-23 10:42:57 +02:00
auxHk.dacEnables[idx] = packet[dataOffset];
dataOffset += 3;
}
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 6; idx++) {
2022-05-23 14:19:57 +02:00
auxHk.dacRawChannelVals[idx] = (packet[dataOffset] << 8) | packet[dataOffset + 1];
2022-05-23 10:42:57 +02:00
dataOffset += 4;
}
2022-01-18 11:41:19 +01:00
2022-05-23 10:42:57 +02:00
auxHk.bootCause = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
2022-05-23 11:25:58 +02:00
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
2022-01-18 11:41:19 +01:00
dataOffset += 6;
2022-05-23 10:42:57 +02:00
coreHk.bootcnt = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
2022-05-23 11:25:58 +02:00
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
2022-01-18 11:41:19 +01:00
dataOffset += 6;
2022-05-23 10:42:57 +02:00
coreHk.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
2022-05-23 11:25:58 +02:00
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
2022-01-18 11:41:19 +01:00
dataOffset += 6;
2022-05-23 10:42:57 +02:00
auxHk.resetCause = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
2022-01-18 11:41:19 +01:00
dataOffset += 4;
2022-05-23 10:42:57 +02:00
coreHk.mpptTime = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
2022-01-18 11:41:19 +01:00
/* +12 because here starts the second csp packet */
dataOffset += 2 + 12;
2022-05-23 10:42:57 +02:00
coreHk.mpptPeriod = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
2022-01-18 11:41:19 +01:00
dataOffset += 4;
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 8; idx++) {
2022-05-23 10:42:57 +02:00
auxHk.deviceTypes[idx] = packet[dataOffset];
dataOffset += 3;
}
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 8; idx++) {
2022-05-23 10:42:57 +02:00
auxHk.devicesStatus[idx] = packet[dataOffset];
dataOffset += 3;
}
2022-01-18 11:41:19 +01:00
2022-05-23 10:42:57 +02:00
auxHk.wdtCntGnd = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
2022-05-23 11:25:58 +02:00
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
2022-01-18 11:41:19 +01:00
dataOffset += 6;
2022-05-23 10:42:57 +02:00
auxHk.wdtGndLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
2022-05-23 11:25:58 +02:00
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
2022-01-18 11:41:19 +01:00
dataOffset += 6;
2022-05-23 14:04:46 +02:00
coreHk.setValidity(true, true);
auxHk.setValidity(true, true);
2022-05-23 10:42:57 +02:00
return RETURN_OK;
2021-02-12 14:31:43 +01:00
}
2022-01-18 11:41:19 +01:00
ReturnValue_t ACUHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
2022-04-04 17:16:52 +02:00
using namespace P60System;
2022-05-23 10:42:57 +02:00
localDataPoolMap.emplace(pool::ACU_CURRENT_IN_CHANNELS, new PoolEntry<int16_t>(6));
localDataPoolMap.emplace(pool::ACU_VOLTAGE_IN_CHANNELS, new PoolEntry<uint16_t>(6));
2022-04-04 17:16:52 +02:00
localDataPoolMap.emplace(pool::ACU_VCC, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(pool::ACU_VBAT, new PoolEntry<uint16_t>({0}));
2022-06-03 09:33:08 +02:00
localDataPoolMap.emplace(pool::ACU_TEMPERATURES, new PoolEntry<float>(3));
2022-04-04 17:16:52 +02:00
localDataPoolMap.emplace(pool::ACU_MPPT_MODE, new PoolEntry<uint8_t>({0}));
2022-05-23 10:42:57 +02:00
localDataPoolMap.emplace(pool::ACU_VBOOST_IN_CHANNELS, new PoolEntry<uint16_t>(6));
localDataPoolMap.emplace(pool::ACU_POWER_IN_CHANNELS, new PoolEntry<uint16_t>(6));
2022-04-04 17:16:52 +02:00
2022-05-23 10:42:57 +02:00
localDataPoolMap.emplace(pool::ACU_DAC_ENABLES, new PoolEntry<uint8_t>(3));
localDataPoolMap.emplace(pool::ACU_DAC_RAW_CHANNELS, new PoolEntry<uint16_t>(6));
2022-04-04 17:16:52 +02:00
localDataPoolMap.emplace(pool::ACU_BOOTCAUSE, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::ACU_BOOTCNT, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::ACU_UPTIME, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::ACU_RESET_CAUSE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(pool::ACU_MPPT_TIME, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(pool::ACU_MPPT_PERIOD, new PoolEntry<uint16_t>({0}));
2022-05-23 10:42:57 +02:00
localDataPoolMap.emplace(pool::ACU_DEVICES, new PoolEntry<uint8_t>(8));
localDataPoolMap.emplace(pool::ACU_DEVICES_STATUS, new PoolEntry<uint8_t>(8));
2022-04-04 17:16:52 +02:00
localDataPoolMap.emplace(pool::ACU_WDT_CNT_GND, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::ACU_WDT_GND_LEFT, new PoolEntry<uint32_t>({0}));
2022-01-18 11:41:19 +01:00
2022-08-15 11:57:57 +02:00
poolManager.subscribeForDiagPeriodicPacket(
subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), false, 10.0));
poolManager.subscribeForRegularPeriodicPacket(
subdp::RegularHkPeriodicParams(auxHk.getSid(), false, 30.0));
2022-01-18 11:41:19 +01:00
return HasReturnvaluesIF::RETURN_OK;
2021-02-12 14:31:43 +01:00
}
2021-12-17 10:45:55 +01:00
void ACUHandler::printChannelStats() {
2022-05-23 10:42:57 +02:00
PoolReadGuard pg(&coreHk);
2022-01-18 11:41:19 +01:00
sif::info << "ACU Info: Current [mA], Voltage [mV]" << std::endl;
2022-05-23 11:25:58 +02:00
for (size_t idx = 0; idx < 6; idx++) {
2022-05-23 10:42:57 +02:00
sif::info << std::setw(8) << std::left << "Channel " << idx << std::dec << "| "
<< static_cast<unsigned int>(coreHk.currentInChannels[idx]) << std::setw(15)
<< std::right << coreHk.voltageInChannels[idx] << std::endl;
}
2021-12-17 10:45:55 +01:00
}
2022-04-26 10:37:25 +02:00
void ACUHandler::setDebugMode(bool enable) { this->debugMode = enable; }
2022-05-23 11:25:58 +02:00
ReturnValue_t ACUHandler::printStatus(DeviceCommandId_t cmd) {
ReturnValue_t result = RETURN_OK;
switch (cmd) {
case (GOMSPACE::PRINT_SWITCH_V_I): {
PoolReadGuard pg(&coreHk);
result = pg.getReadResult();
if (result != HasReturnvaluesIF::RETURN_OK) {
break;
}
printChannelStats();
break;
}
default: {
return DeviceHandlerIF::COMMAND_NOT_SUPPORTED;
}
}
if (result != HasReturnvaluesIF::RETURN_OK) {
sif::warning << "Reading PDU1 HK table failed!" << std::endl;
}
return result;
}