eive-obsw/mission/power/P60DockHandler.cpp

274 lines
11 KiB
C++
Raw Normal View History

2022-01-17 15:58:27 +01:00
#include <fsfw/datapool/PoolReadGuard.h>
2023-03-26 16:13:54 +02:00
#include <mission/power/P60DockHandler.h>
2022-01-17 15:58:27 +01:00
2021-02-12 10:20:39 +01:00
#include "OBSWConfig.h"
2021-02-06 11:57:45 +01:00
2022-04-28 15:58:31 +02:00
P60DockHandler::P60DockHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie,
2023-03-26 15:28:00 +02:00
FailureIsolationBase *customFdir, bool enableHkSets)
: GomspaceDeviceHandler(objectId, comIF, comCookie, cfg, customFdir, enableHkSets),
2022-04-07 12:22:08 +02:00
coreHk(this),
2022-08-26 14:28:06 +02:00
auxHk(this) {
cfg.maxConfigTableAddress = P60Dock::MAX_CONFIGTABLE_ADDRESS;
cfg.maxHkTableAddress = P60Dock::MAX_HKTABLE_ADDRESS;
cfg.hkTableSize = P60Dock::HK_TABLE_SIZE;
cfg.cfgTableSize = P60Dock::CONFIG_TABLE_SIZE;
}
2021-01-28 14:55:21 +01:00
2022-01-17 15:58:27 +01:00
P60DockHandler::~P60DockHandler() {}
2021-01-28 14:55:21 +01:00
2022-01-17 15:58:27 +01:00
ReturnValue_t P60DockHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = GOMSPACE::REQUEST_HK_TABLE;
return buildCommandFromCommand(*id, NULL, 0);
2021-02-12 10:20:39 +01:00
}
2021-01-28 14:55:21 +01:00
void P60DockHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {
2022-01-17 15:58:27 +01:00
parseHkTableReply(packet);
2021-01-28 14:55:21 +01:00
}
2021-02-09 10:31:40 +01:00
2021-02-12 10:20:39 +01:00
void P60DockHandler::parseHkTableReply(const uint8_t *packet) {
2022-04-04 17:16:52 +02:00
using namespace P60Dock;
2022-04-07 12:22:08 +02:00
PoolReadGuard pg0(&coreHk);
PoolReadGuard pg1(&auxHk);
2022-08-26 14:28:06 +02:00
if (pg0.getReadResult() != returnvalue::OK or pg1.getReadResult() != returnvalue::OK) {
2022-04-07 12:22:08 +02:00
coreHk.setValidity(false, true);
auxHk.setValidity(false, true);
2022-04-04 17:16:52 +02:00
return;
}
2022-01-17 15:58:27 +01:00
/**
* Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table
* address.
*/
2022-04-04 17:16:52 +02:00
for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
2022-08-26 14:28:06 +02:00
coreHk.currents[idx] = as<int16_t>(packet + (idx * 2));
2022-04-04 17:16:52 +02:00
}
for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
2022-08-26 14:28:06 +02:00
coreHk.voltages[idx] = as<uint16_t>(packet + 0x1a + (idx * 2));
2022-04-04 17:16:52 +02:00
}
for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
coreHk.outputEnables[idx] = *(packet + 0x34 + idx);
2022-04-04 17:16:52 +02:00
}
2022-01-17 15:58:27 +01:00
coreHk.temperature1 = as<int16_t>(packet + 0x44) * 0.1;
coreHk.temperature2 = as<int16_t>(packet + 0x44 + 2) * 0.1;
2022-01-17 15:58:27 +01:00
auxHk.bootcause = as<uint32_t>(packet + 0x48);
coreHk.bootCount = as<uint32_t>(packet + 0x4c);
2022-04-07 19:48:09 +02:00
if (firstHk) {
2022-04-07 13:11:20 +02:00
triggerEvent(P60_BOOT_COUNT, coreHk.bootCount.value);
}
auxHk.uptime = as<uint32_t>(packet + 0x50);
auxHk.resetcause = as<uint16_t>(packet + 0x54);
uint8_t newBattMode = packet[0x56];
if (firstHk) {
triggerEvent(BATT_MODE, newBattMode);
} else if (newBattMode != coreHk.battMode.value) {
triggerEvent(BATT_MODE_CHANGED, coreHk.battMode.value, newBattMode);
}
coreHk.battMode = newBattMode;
2023-05-16 11:36:19 +02:00
auxHk.heaterForBp4PackOn = *(packet + 0x57);
auxHk.converter5VStatus = *(packet + 0x58);
2022-01-17 15:58:27 +01:00
2022-04-04 17:16:52 +02:00
for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
2022-08-26 14:28:06 +02:00
auxHk.latchups[idx] = as<uint16_t>(packet + 0x5a + (idx * 2));
2022-04-04 17:16:52 +02:00
}
2022-01-17 15:58:27 +01:00
auxHk.dockVbatVoltageValue = as<uint16_t>(packet + 0x74);
auxHk.dockVccCurrent = as<int16_t>(packet + 0x76);
coreHk.batteryCurrent = as<int16_t>(packet + 0x78);
coreHk.batteryVoltage = as<uint16_t>(packet + 0x7a);
2022-01-17 15:58:27 +01:00
auxHk.batteryTemperature1 = as<int16_t>(packet + 0x7c);
auxHk.batteryTemperature2 = as<int16_t>(packet + 0x7c + 2);
2022-01-17 15:58:27 +01:00
2022-04-04 17:16:52 +02:00
for (uint8_t idx = 0; idx < NUM_DEVS; idx++) {
auxHk.devicesType[idx] = *(packet + 0x80 + idx);
2022-04-04 17:16:52 +02:00
}
for (uint8_t idx = 0; idx < NUM_DEVS; idx++) {
auxHk.devicesStatus[idx] = *(packet + 0x88 + idx);
2022-04-04 17:16:52 +02:00
}
2022-01-17 15:58:27 +01:00
auxHk.dearmStatus = *(packet + 0x90);
auxHk.wdtCntGnd = as<uint32_t>(packet + 0x94);
auxHk.wdtCntI2c = as<uint32_t>(packet + 0x98);
auxHk.wdtCntCan = as<uint32_t>(packet + 0x9c);
auxHk.wdtCntCsp1 = as<uint32_t>(packet + 0xa0);
auxHk.wdtCntCsp2 = as<uint32_t>(packet + 0xa0 + 4);
auxHk.wdtGndLeft = as<uint32_t>(packet + 0xa8);
auxHk.wdtI2cLeft = as<uint32_t>(packet + 0xac);
auxHk.wdtCanLeft = as<uint32_t>(packet + 0xb0);
auxHk.wdtCspLeft1 = *(packet + 0xb4);
auxHk.wdtCspLeft2 = *(packet + 0xb4 + 1);
auxHk.batteryChargeCurrent = as<int16_t>(packet + 0xb6);
auxHk.batteryDischargeCurrent = as<int16_t>(packet + 0xb8);
auxHk.ant6Depl = *(packet + 0xba);
auxHk.ar6Depl = *(packet + 0xbb);
if (firstHk) {
firstHk = false;
}
2022-04-07 12:22:08 +02:00
coreHk.setValidity(true, true);
auxHk.setValidity(true, true);
2023-05-16 11:36:19 +02:00
// No BP4 pack, no this is always invalid.
auxHk.heaterForBp4PackOn.setValid(false);
2021-02-12 10:20:39 +01:00
}
2022-01-17 15:58:27 +01:00
ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) {
2022-08-27 01:02:08 +02:00
using namespace P60Dock;
2022-04-07 19:48:09 +02:00
localDataPoolMap.emplace(pool::P60_CURRENTS, &hkCurrents);
2022-04-04 17:16:52 +02:00
2022-04-07 19:48:09 +02:00
localDataPoolMap.emplace(pool::P60_VOLTAGES, &hkVoltages);
2022-04-04 17:16:52 +02:00
2022-04-07 19:48:09 +02:00
localDataPoolMap.emplace(pool::P60_OUTPUT_ENABLE, &outputEnables);
2022-04-04 17:16:52 +02:00
2022-06-03 18:03:25 +02:00
localDataPoolMap.emplace(pool::P60DOCK_TEMPERATURE_1, new PoolEntry<float>({0}));
localDataPoolMap.emplace(pool::P60DOCK_TEMPERATURE_2, new PoolEntry<float>({0}));
2022-04-04 17:16:52 +02:00
localDataPoolMap.emplace(pool::P60DOCK_BOOT_CAUSE, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_BOOT_CNT, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_UPTIME, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_RESETCAUSE, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_BATT_MODE, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_HEATER_ON, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_CONV_5V_ENABLE_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(pool::LATCHUPS, &latchups);
localDataPoolMap.emplace(pool::P60DOCK_DOCK_VBAT, new PoolEntry<uint16_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_DOCK_VCC_CURRENT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_BATTERY_CURRENT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_BATTERY_VOLTAGE, new PoolEntry<uint16_t>({0}));
2022-06-03 18:03:25 +02:00
localDataPoolMap.emplace(pool::P60DOCK_BATTERY_TEMPERATURE_1, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_BATTERY_TEMPERATURE_2, new PoolEntry<int16_t>({0}));
2022-04-04 17:16:52 +02:00
localDataPoolMap.emplace(pool::DEVICES_TYPE, &devicesType);
localDataPoolMap.emplace(pool::DEVICES_STATUS, &devicesStatus);
localDataPoolMap.emplace(pool::P60DOCK_DEARM_STATUS, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_GND, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_I2C, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_CAN, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_CSP_1, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_CNT_CSP_2, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_GND_LEFT, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_I2C_LEFT, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_CAN_LEFT, new PoolEntry<uint32_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_CSP_LEFT_1, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_WDT_CSP_LEFT_2, new PoolEntry<uint8_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_BATT_CHARGE_CURRENT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_BATT_DISCHARGE_CURRENT, new PoolEntry<int16_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_ANT6_DEPL, new PoolEntry<int8_t>({0}));
localDataPoolMap.emplace(pool::P60DOCK_AR6_DEPL, new PoolEntry<int8_t>({0}));
2022-08-15 11:57:57 +02:00
poolManager.subscribeForDiagPeriodicPacket(
2023-04-04 19:18:53 +02:00
subdp::DiagnosticsHkPeriodicParams(coreHk.getSid(), enableHkSets, 30.0));
2022-08-15 11:57:57 +02:00
poolManager.subscribeForRegularPeriodicPacket(
2023-04-04 19:18:53 +02:00
subdp::RegularHkPeriodicParams(auxHk.getSid(), enableHkSets, 6000.0));
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2021-01-28 14:55:21 +01:00
}
2021-09-20 11:47:19 +02:00
ReturnValue_t P60DockHandler::printStatus(DeviceCommandId_t cmd) {
2022-08-24 17:27:47 +02:00
ReturnValue_t result = returnvalue::OK;
2022-01-17 15:58:27 +01:00
switch (cmd) {
case (GOMSPACE::PRINT_SWITCH_V_I): {
2022-04-07 12:22:08 +02:00
PoolReadGuard pg0(&coreHk);
PoolReadGuard pg1(&auxHk);
2022-08-26 14:28:06 +02:00
if (pg0.getReadResult() != returnvalue::OK or pg1.getReadResult() != returnvalue::OK) {
break;
}
printHkTableSwitchIV();
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
}
case (GOMSPACE::PRINT_LATCHUPS): {
2022-04-07 12:22:08 +02:00
PoolReadGuard pg(&auxHk);
result = pg.getReadResult();
printHkTableLatchups();
2022-08-24 17:27:47 +02:00
if (result != returnvalue::OK) {
break;
2022-01-17 15:58:27 +01:00
}
2022-08-24 17:27:47 +02:00
return returnvalue::OK;
2021-09-20 11:47:19 +02:00
}
2022-01-17 15:58:27 +01:00
default: {
2022-05-23 11:25:58 +02:00
return DeviceHandlerIF::COMMAND_NOT_SUPPORTED;
2021-09-20 11:47:19 +02:00
}
2022-01-17 15:58:27 +01:00
}
sif::warning << "Reading P60 Dock HK table failed" << std::endl;
2022-08-24 17:27:47 +02:00
return returnvalue::FAILED;
2021-09-20 11:47:19 +02:00
}
void P60DockHandler::printHkTableSwitchIV() {
2022-04-04 17:16:52 +02:00
using namespace P60Dock;
sif::info << "P60 Dock Info:" << std::endl;
2022-04-07 12:22:08 +02:00
sif::info << "Boot Cause: " << auxHk.bootcause << " | Boot Count: " << std::setw(4) << std::right
<< coreHk.bootCount << std::endl;
sif::info << "Reset Cause: " << auxHk.resetcause
<< " | Battery Mode: " << static_cast<int>(coreHk.battMode.value) << std::endl;
sif::info << "SwitchState, Currents [mA], Voltages [mV]:" << std::endl;
sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "Dock VBAT VCC" << std::dec
2022-04-07 12:22:08 +02:00
<< "| -, " << std::setw(4) << std::right << auxHk.dockVccCurrent << ", " << std::setw(5)
<< auxHk.dockVbatVoltageValue << std::endl;
sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << "BATT" << std::dec << "| -, "
2022-04-07 12:22:08 +02:00
<< std::setw(4) << std::right << coreHk.batteryCurrent.value << ", " << std::setw(5)
<< coreHk.batteryVoltage.value << std::endl;
2022-04-04 17:16:52 +02:00
auto genericPrintoutHandler = [&](std::string name, uint8_t idx) {
sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << name << std::dec << "| "
2022-04-07 12:22:08 +02:00
<< unsigned(coreHk.outputEnables[idx]) << ", " << std::setw(4) << std::right
<< coreHk.currents[idx] << ", " << std::setw(5) << coreHk.voltages[idx] << std::endl;
2022-04-04 17:16:52 +02:00
};
genericPrintoutHandler("ACU VCC", hk::ACU_VCC);
genericPrintoutHandler("ACU VBAT", hk::ACU_VBAT);
genericPrintoutHandler("PDU1 VCC", hk::PDU1_VCC);
genericPrintoutHandler("PDU1 VBAT", hk::PDU1_VBAT);
genericPrintoutHandler("PDU2 VCC", hk::PDU2_VCC);
genericPrintoutHandler("PDU2 VBAT", hk::PDU2_VBAT);
genericPrintoutHandler("Stack VBAT", hk::STACK_VBAT);
genericPrintoutHandler("Stack 3V3", hk::STACK_3V3);
genericPrintoutHandler("Stack 5V", hk::STACK_5V);
2021-09-20 11:47:19 +02:00
}
2022-04-07 12:22:08 +02:00
LocalPoolDataSetBase *P60DockHandler::getDataSetHandle(sid_t sid) {
if (sid == coreHk.getSid()) {
return &coreHk;
} else if (sid == auxHk.getSid()) {
return &auxHk;
}
return nullptr;
}
void P60DockHandler::printHkTableLatchups() {
2022-04-04 17:16:52 +02:00
using namespace P60Dock;
sif::info << "P60 Latchup Information" << std::endl;
2022-04-04 17:16:52 +02:00
auto genericPrintoutHandler = [&](std::string name, uint8_t idx) {
sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << name << std::dec << "| "
2022-04-07 12:22:08 +02:00
<< std::setw(4) << std::right << auxHk.latchups[idx] << std::endl;
2022-04-04 17:16:52 +02:00
};
genericPrintoutHandler("ACU VCC", hk::ACU_VCC);
genericPrintoutHandler("ACU VBAT", hk::ACU_VBAT);
genericPrintoutHandler("PDU1 VCC", hk::PDU1_VCC);
genericPrintoutHandler("PDU1 VBAT", hk::PDU1_VBAT);
genericPrintoutHandler("PDU2 VCC", hk::PDU2_VCC);
genericPrintoutHandler("PDU2 VBAT", hk::PDU2_VBAT);
genericPrintoutHandler("STACK VBAT", hk::STACK_VBAT);
genericPrintoutHandler("STACK 3V3", hk::STACK_3V3);
genericPrintoutHandler("STACK 5V", hk::STACK_5V);
genericPrintoutHandler("GS 3V3", hk::GS3V3);
genericPrintoutHandler("GS 5V", hk::GS5V);
genericPrintoutHandler("X3 VBAT", hk::X3_IDLE_VBAT);
genericPrintoutHandler("X3 VCC", hk::X3_IDLE_VCC);
}
2022-04-26 10:37:25 +02:00
void P60DockHandler::setDebugMode(bool enable) { this->debugMode = enable; }
2022-08-27 01:02:08 +02:00
void P60DockHandler::letChildHandleConfigReply(DeviceCommandId_t id, const uint8_t *packet) {
2022-08-30 23:54:05 +02:00
handleDeviceTm(packet, P60Dock::CONFIG_TABLE_SIZE, id);
2022-08-27 01:02:08 +02:00
}