327 lines
14 KiB
C++
327 lines
14 KiB
C++
#include "P60DockHandler.h"
|
|
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
|
|
#include "OBSWConfig.h"
|
|
|
|
P60DockHandler::P60DockHandler(object_id_t objectId, object_id_t comIF, CookieIF *comCookie,
|
|
FailureIsolationBase *customFdir)
|
|
: GomspaceDeviceHandler(objectId, comIF, comCookie, customFdir,
|
|
P60Dock::MAX_CONFIGTABLE_ADDRESS, P60Dock::MAX_HKTABLE_ADDRESS,
|
|
P60Dock::HK_TABLE_REPLY_SIZE),
|
|
coreHk(this),
|
|
auxHk(this) {}
|
|
|
|
P60DockHandler::~P60DockHandler() {}
|
|
|
|
ReturnValue_t P60DockHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
|
|
*id = GOMSPACE::REQUEST_HK_TABLE;
|
|
return requestHkTableAction.handle();
|
|
}
|
|
|
|
void P60DockHandler::letChildHandleHkReply(DeviceCommandId_t id, const uint8_t *packet) {
|
|
parseHkTableReply(packet);
|
|
/**
|
|
* Hk table will be sent to the commander if hk table request was not triggered by the
|
|
* P60DockHandler itself.
|
|
*/
|
|
handleDeviceTM(&coreHk, id, true);
|
|
}
|
|
|
|
void P60DockHandler::parseHkTableReply(const uint8_t *packet) {
|
|
using namespace P60Dock;
|
|
uint16_t dataOffset = 0;
|
|
PoolReadGuard pg0(&coreHk);
|
|
PoolReadGuard pg1(&auxHk);
|
|
if (pg0.getReadResult() != HasReturnvaluesIF::RETURN_OK or
|
|
pg1.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
|
|
coreHk.setValidity(false, true);
|
|
auxHk.setValidity(false, true);
|
|
return;
|
|
}
|
|
/**
|
|
* Fist 10 bytes contain the gomspace header. Each variable is preceded by the 16-bit table
|
|
* address.
|
|
*/
|
|
dataOffset += 12;
|
|
for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
|
|
coreHk.currents[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
}
|
|
for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
|
|
coreHk.voltages[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
}
|
|
for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
|
|
coreHk.outputEnables[idx] = *(packet + dataOffset);
|
|
dataOffset += 3;
|
|
}
|
|
|
|
coreHk.temperature1 =
|
|
static_cast<int16_t>(*(packet + dataOffset) << 8 | *(packet + dataOffset + 1)) * 0.1;
|
|
dataOffset += 4;
|
|
coreHk.temperature2 =
|
|
static_cast<int16_t>(*(packet + dataOffset) << 8 | *(packet + dataOffset + 1)) * 0.1;
|
|
dataOffset += 4;
|
|
|
|
auxHk.bootcause = *(packet + dataOffset) << 24 |
|
|
|
|
*(packet + dataOffset + 1) << 16 | *(packet + dataOffset + 2) << 8 |
|
|
*(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
coreHk.bootCount = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
if (firstHk) {
|
|
triggerEvent(P60_BOOT_COUNT, coreHk.bootCount.value);
|
|
}
|
|
dataOffset += 6;
|
|
auxHk.uptime = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
auxHk.resetcause = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
|
|
dataOffset += 4;
|
|
uint8_t newBattMode = packet[dataOffset];
|
|
if (firstHk) {
|
|
triggerEvent(BATT_MODE, newBattMode);
|
|
} else if (newBattMode != coreHk.battMode.value) {
|
|
triggerEvent(BATT_MODE_CHANGED, coreHk.battMode.value, newBattMode);
|
|
}
|
|
coreHk.battMode = newBattMode;
|
|
|
|
dataOffset += 3;
|
|
auxHk.heaterOn = *(packet + dataOffset);
|
|
/* + 13 because here begins a new gomspace csp data field */
|
|
dataOffset += 13;
|
|
auxHk.converter5VStatus = *(packet + dataOffset);
|
|
dataOffset += 3;
|
|
|
|
for (uint8_t idx = 0; idx < hk::CHNLS_LEN; idx++) {
|
|
auxHk.latchups[idx] = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
}
|
|
|
|
auxHk.dockVbatVoltageValue = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
auxHk.dockVccCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
coreHk.batteryCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
coreHk.batteryVoltage = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
|
|
auxHk.batteryTemperature1 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
auxHk.batteryTemperature2 = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
|
|
for (uint8_t idx = 0; idx < NUM_DEVS; idx++) {
|
|
auxHk.devicesType[idx] = *(packet + dataOffset);
|
|
dataOffset += 3;
|
|
}
|
|
for (uint8_t idx = 0; idx < NUM_DEVS; idx++) {
|
|
auxHk.devicesStatus[idx] = *(packet + dataOffset);
|
|
dataOffset += 3;
|
|
}
|
|
|
|
auxHk.dearmStatus = *(packet + dataOffset);
|
|
dataOffset += 3;
|
|
|
|
auxHk.wdtCntGnd = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
auxHk.wdtCntI2c = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
auxHk.wdtCntCan = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
auxHk.wdtCntCsp1 = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
auxHk.wdtCntCsp2 = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
|
|
auxHk.wdtGndLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
auxHk.wdtI2cLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
dataOffset += 6;
|
|
auxHk.wdtCanLeft = *(packet + dataOffset) << 24 | *(packet + dataOffset + 1) << 16 |
|
|
*(packet + dataOffset + 2) << 8 | *(packet + dataOffset + 3);
|
|
/* +16 because here begins a new gomspace csp packet */
|
|
dataOffset += 16;
|
|
|
|
auxHk.wdtCspLeft1 = *(packet + dataOffset);
|
|
dataOffset += 3;
|
|
auxHk.wdtCspLeft2 = *(packet + dataOffset);
|
|
dataOffset += 3;
|
|
|
|
auxHk.batteryChargeCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
auxHk.batteryDischargeCurrent = *(packet + dataOffset) << 8 | *(packet + dataOffset + 1);
|
|
dataOffset += 4;
|
|
auxHk.ant6Depl = *(packet + dataOffset);
|
|
dataOffset += 3;
|
|
auxHk.ar6Depl = *(packet + dataOffset);
|
|
if (firstHk) {
|
|
firstHk = false;
|
|
}
|
|
coreHk.setValidity(true, true);
|
|
auxHk.setValidity(true, true);
|
|
}
|
|
|
|
ReturnValue_t P60DockHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
|
|
LocalDataPoolManager &poolManager) {
|
|
using namespace P60System;
|
|
localDataPoolMap.emplace(pool::P60_CURRENTS, &hkCurrents);
|
|
|
|
localDataPoolMap.emplace(pool::P60_VOLTAGES, &hkVoltages);
|
|
|
|
localDataPoolMap.emplace(pool::P60_OUTPUT_ENABLE, &outputEnables);
|
|
|
|
localDataPoolMap.emplace(pool::P60DOCK_TEMPERATURE_1, new PoolEntry<float>({0}));
|
|
localDataPoolMap.emplace(pool::P60DOCK_TEMPERATURE_2, new PoolEntry<float>({0}));
|
|
|
|
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}));
|
|
|
|
localDataPoolMap.emplace(pool::P60DOCK_BATTERY_TEMPERATURE_1, new PoolEntry<int16_t>({0}));
|
|
localDataPoolMap.emplace(pool::P60DOCK_BATTERY_TEMPERATURE_2, new PoolEntry<int16_t>({0}));
|
|
|
|
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}));
|
|
poolManager.subscribeForPeriodicPacket(coreHk.getSid(), false, 10.0, false);
|
|
poolManager.subscribeForPeriodicPacket(auxHk.getSid(), false, 30.0, false);
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t P60DockHandler::printStatus(DeviceCommandId_t cmd) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
switch (cmd) {
|
|
case (GOMSPACE::PRINT_SWITCH_V_I): {
|
|
PoolReadGuard pg0(&coreHk);
|
|
PoolReadGuard pg1(&auxHk);
|
|
if (pg0.getReadResult() != HasReturnvaluesIF::RETURN_OK or
|
|
pg1.getReadResult() != HasReturnvaluesIF::RETURN_OK) {
|
|
break;
|
|
}
|
|
printHkTableSwitchIV();
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
case (GOMSPACE::PRINT_LATCHUPS): {
|
|
PoolReadGuard pg(&auxHk);
|
|
result = pg.getReadResult();
|
|
printHkTableLatchups();
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
break;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
default: {
|
|
return DeviceHandlerIF::COMMAND_NOT_SUPPORTED;
|
|
}
|
|
}
|
|
sif::warning << "Reading P60 Dock HK table failed" << std::endl;
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
void P60DockHandler::printHkTableSwitchIV() {
|
|
using namespace P60Dock;
|
|
sif::info << "P60 Dock Info:" << std::endl;
|
|
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
|
|
<< "| -, " << 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 << "| -, "
|
|
<< std::setw(4) << std::right << coreHk.batteryCurrent.value << ", " << std::setw(5)
|
|
<< coreHk.batteryVoltage.value << std::endl;
|
|
|
|
auto genericPrintoutHandler = [&](std::string name, uint8_t idx) {
|
|
sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << name << std::dec << "| "
|
|
<< unsigned(coreHk.outputEnables[idx]) << ", " << std::setw(4) << std::right
|
|
<< coreHk.currents[idx] << ", " << std::setw(5) << coreHk.voltages[idx] << std::endl;
|
|
};
|
|
|
|
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);
|
|
}
|
|
|
|
LocalPoolDataSetBase *P60DockHandler::getDataSetHandle(sid_t sid) {
|
|
if (sid == coreHk.getSid()) {
|
|
return &coreHk;
|
|
} else if (sid == auxHk.getSid()) {
|
|
return &auxHk;
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
void P60DockHandler::printHkTableLatchups() {
|
|
using namespace P60Dock;
|
|
sif::info << "P60 Latchup Information" << std::endl;
|
|
auto genericPrintoutHandler = [&](std::string name, uint8_t idx) {
|
|
sif::info << std::setw(MAX_CHANNEL_STR_WIDTH) << std::left << name << std::dec << "| "
|
|
<< std::setw(4) << std::right << auxHk.latchups[idx] << std::endl;
|
|
};
|
|
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);
|
|
}
|
|
|
|
void P60DockHandler::setDebugMode(bool enable) { this->debugMode = enable; }
|