295 lines
9.5 KiB
C++
295 lines
9.5 KiB
C++
#include <fsfw/datapool/PoolReadGuard.h>
|
|
#include <mission/power/BpxBatteryHandler.h>
|
|
|
|
BpxBatteryHandler::BpxBatteryHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCookie,
|
|
bool enableHkSets)
|
|
: DeviceHandlerBase(objectId, comIF, comCookie),
|
|
enableHkSets(enableHkSets),
|
|
hkSet(this),
|
|
cfgSet(this) {}
|
|
|
|
BpxBatteryHandler::~BpxBatteryHandler() {}
|
|
|
|
void BpxBatteryHandler::doStartUp() {
|
|
if (state == States::CHECK_COM) {
|
|
if (commandExecuted) {
|
|
state = States::IDLE;
|
|
commandExecuted = false;
|
|
if (goToNormalModeImmediately) {
|
|
setMode(MODE_NORMAL);
|
|
} else {
|
|
setMode(_MODE_TO_ON);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void BpxBatteryHandler::doShutDown() {
|
|
// Perform a COM check on reboot
|
|
state = States::CHECK_COM;
|
|
setMode(MODE_OFF);
|
|
}
|
|
|
|
ReturnValue_t BpxBatteryHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
|
|
*id = bpxBat::GET_HK;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
|
|
ReturnValue_t BpxBatteryHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
|
|
if (state == States::CHECK_COM) {
|
|
*id = bpxBat::PING;
|
|
return buildCommandFromCommand(*id, nullptr, 0);
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void BpxBatteryHandler::fillCommandAndReplyMap() {
|
|
using namespace bpxBat;
|
|
insertInCommandAndReplyMap(GET_HK, 1, &hkSet, GET_HK_REPLY_LEN);
|
|
insertInCommandAndReplyMap(bpxBat::PING, 1, nullptr, PING_REPLY_LEN);
|
|
insertInCommandAndReplyMap(bpxBat::REBOOT, 1, nullptr, 0);
|
|
insertInCommandAndReplyMap(bpxBat::RESET_COUNTERS, 1, nullptr, EMPTY_REPLY_LEN);
|
|
insertInCommandAndReplyMap(bpxBat::CONFIG_CMD, 1, nullptr, EMPTY_REPLY_LEN);
|
|
insertInCommandAndReplyMap(bpxBat::CONFIG_GET, 1, &cfgSet, CONFIG_GET_REPLY_LEN);
|
|
}
|
|
|
|
ReturnValue_t BpxBatteryHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
|
const uint8_t* commandData,
|
|
size_t commandDataLen) {
|
|
switch (deviceCommand) {
|
|
case (bpxBat::GET_HK): {
|
|
cmdBuf[0] = bpxBat::PORT_GET_HK;
|
|
this->rawPacketLen = 1;
|
|
break;
|
|
}
|
|
case (bpxBat::PING): {
|
|
if (commandDataLen == 1 and commandData != nullptr) {
|
|
sentPingByte = commandData[0];
|
|
} else {
|
|
sentPingByte = bpxBat::DEFAULT_PING_SENT_BYTE;
|
|
}
|
|
|
|
cmdBuf[0] = bpxBat::PORT_PING;
|
|
cmdBuf[1] = sentPingByte;
|
|
this->rawPacketLen = 2;
|
|
break;
|
|
}
|
|
case (bpxBat::REBOOT): {
|
|
sif::info << "BPX BATT: Executing reboot command" << std::endl;
|
|
cmdBuf[0] = bpxBat::PORT_REBOOT;
|
|
cmdBuf[1] = 0x80;
|
|
cmdBuf[2] = 0x07;
|
|
cmdBuf[3] = 0x80;
|
|
cmdBuf[4] = 0x07;
|
|
this->rawPacketLen = 5;
|
|
// This instructs the FDIR to set the device mode off and on again
|
|
// to ensure the I2C communication is also verified
|
|
triggerEvent(DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT);
|
|
break;
|
|
}
|
|
case (bpxBat::RESET_COUNTERS): {
|
|
cmdBuf[0] = bpxBat::PORT_RESET_COUNTERS;
|
|
cmdBuf[1] = bpxBat::RESET_COUNTERS_MAGIC_VALUE;
|
|
this->rawPacketLen = 2;
|
|
break;
|
|
}
|
|
case (bpxBat::CONFIG_CMD): {
|
|
cmdBuf[0] = bpxBat::PORT_CONFIG_CMD;
|
|
// Needs to be set to 0x01 according to datasheet
|
|
cmdBuf[1] = 0x01;
|
|
this->rawPacketLen = 2;
|
|
break;
|
|
}
|
|
case (bpxBat::CONFIG_GET): {
|
|
cmdBuf[0] = bpxBat::PORT_CONFIG_GET;
|
|
this->rawPacketLen = 1;
|
|
break;
|
|
}
|
|
case (bpxBat::CONFIG_SET): {
|
|
cmdBuf[0] = bpxBat::PORT_CONFIG_SET;
|
|
if (commandDataLen != 3) {
|
|
return DeviceHandlerIF::INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
|
|
}
|
|
for (uint8_t idx = 0; idx < 3; idx++) {
|
|
cmdBuf[idx + 1] = commandData[idx];
|
|
}
|
|
this->rawPacketLen = 4;
|
|
break;
|
|
}
|
|
case (bpxBat::MAN_HEAT_ON): {
|
|
cmdBuf[0] = bpxBat::PORT_MAN_HEAT_ON;
|
|
if (commandDataLen != 2) {
|
|
return DeviceHandlerIF::INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
|
|
}
|
|
for (uint8_t idx = 0; idx < 2; idx++) {
|
|
cmdBuf[idx + 1] = commandData[idx];
|
|
}
|
|
this->rawPacketLen = 3;
|
|
break;
|
|
}
|
|
case (bpxBat::MAN_HEAT_OFF): {
|
|
cmdBuf[0] = bpxBat::PORT_MAN_HEAT_OFF;
|
|
this->rawPacketLen = 1;
|
|
break;
|
|
}
|
|
default: {
|
|
return DeviceHandlerIF::COMMAND_NOT_SUPPORTED;
|
|
}
|
|
}
|
|
this->rawPacket = cmdBuf.data();
|
|
|
|
lastCmd = deviceCommand;
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t BpxBatteryHandler::scanForReply(const uint8_t* start, size_t remainingSize,
|
|
DeviceCommandId_t* foundId, size_t* foundLen) {
|
|
using namespace bpxBat;
|
|
switch (lastCmd) {
|
|
case (bpxBat::GET_HK): {
|
|
if (remainingSize != GET_HK_REPLY_LEN) {
|
|
return DeviceHandlerIF::LENGTH_MISSMATCH;
|
|
}
|
|
break;
|
|
}
|
|
case (bpxBat::PING):
|
|
case (bpxBat::MAN_HEAT_ON):
|
|
case (bpxBat::MAN_HEAT_OFF): {
|
|
if (remainingSize != PING_REPLY_LEN) {
|
|
return DeviceHandlerIF::LENGTH_MISSMATCH;
|
|
}
|
|
break;
|
|
}
|
|
case (bpxBat::REBOOT): {
|
|
// Ignore
|
|
break;
|
|
}
|
|
case (bpxBat::RESET_COUNTERS):
|
|
case (bpxBat::CONFIG_CMD):
|
|
case (bpxBat::CONFIG_SET): {
|
|
if (remainingSize != EMPTY_REPLY_LEN) {
|
|
return DeviceHandlerIF::LENGTH_MISSMATCH;
|
|
}
|
|
break;
|
|
}
|
|
case (bpxBat::CONFIG_GET): {
|
|
if (remainingSize != CONFIG_GET_REPLY_LEN) {
|
|
return DeviceHandlerIF::LENGTH_MISSMATCH;
|
|
}
|
|
|
|
break;
|
|
}
|
|
default: {
|
|
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
|
}
|
|
}
|
|
*foundLen = remainingSize;
|
|
*foundId = lastCmd;
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t BpxBatteryHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) {
|
|
if (id != bpxBat::REBOOT and packet[1] != 0) {
|
|
return DeviceHandlerIF::DEVICE_REPORTED_ERROR;
|
|
}
|
|
switch (id) {
|
|
case (bpxBat::GET_HK): {
|
|
PoolReadGuard rg(&hkSet);
|
|
ReturnValue_t result = hkSet.parseRawHk(packet + 2, 21);
|
|
hkSet.setValidity(true, true);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
if (debugMode) {
|
|
sif::info << "BPX Battery HK output:" << std::endl;
|
|
sif::info << "Charge current [mA]: " << hkSet.chargeCurrent << std::endl;
|
|
sif::info << "Discharge current [mA]: " << hkSet.dischargeCurrent << std::endl;
|
|
sif::info << "Heater current [mA]: " << hkSet.heaterCurrent << std::endl;
|
|
sif::info << "Battery voltage [mV]: " << hkSet.battVoltage << std::endl;
|
|
sif::info << "Battery Temperature 1 [C]: " << hkSet.battTemp1 << std::endl;
|
|
sif::info << "Battery Temperature 2 [C]: " << hkSet.battTemp2 << std::endl;
|
|
sif::info << "Battery Temperature 3 [C]: " << hkSet.battTemp3 << std::endl;
|
|
sif::info << "Battery Temperature 4 [C]: " << hkSet.battTemp4 << std::endl;
|
|
sif::info << "Battery Reboot Counter: " << hkSet.rebootCounter << std::endl;
|
|
sif::info << "Battery Boot Cause: " << static_cast<int>(hkSet.bootcause.value) << std::endl;
|
|
}
|
|
break;
|
|
}
|
|
case (bpxBat::PING): {
|
|
if (packet[2] != sentPingByte) {
|
|
return DeviceHandlerIF::INVALID_DATA;
|
|
}
|
|
if (getMode() == _MODE_START_UP) {
|
|
commandExecuted = true;
|
|
}
|
|
break;
|
|
}
|
|
case (bpxBat::RESET_COUNTERS):
|
|
case (bpxBat::CONFIG_CMD):
|
|
case (bpxBat::CONFIG_SET): {
|
|
break;
|
|
}
|
|
case (bpxBat::MAN_HEAT_ON):
|
|
case (bpxBat::MAN_HEAT_OFF): {
|
|
if (packet[2] != 0x01) {
|
|
return DeviceHandlerIF::DEVICE_DID_NOT_EXECUTE;
|
|
}
|
|
break;
|
|
}
|
|
case (bpxBat::CONFIG_GET): {
|
|
PoolReadGuard rg(&cfgSet);
|
|
ReturnValue_t result = cfgSet.parseRawHk(packet + 2, 3);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
cfgSet.setValidity(true, true);
|
|
break;
|
|
}
|
|
case (bpxBat::REBOOT): {
|
|
break;
|
|
}
|
|
default: {
|
|
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
|
}
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
uint32_t BpxBatteryHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 10000; }
|
|
|
|
ReturnValue_t BpxBatteryHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
|
LocalDataPoolManager& poolManager) {
|
|
localDataPoolMap.emplace(bpxBat::BATT_TEMP_1, &battTemp1);
|
|
localDataPoolMap.emplace(bpxBat::BATT_TEMP_2, &battTemp2);
|
|
localDataPoolMap.emplace(bpxBat::BATT_TEMP_3, &battTemp3);
|
|
localDataPoolMap.emplace(bpxBat::BATT_TEMP_4, &battTemp4);
|
|
localDataPoolMap.emplace(bpxBat::CHARGE_CURRENT, &chargeCurrent);
|
|
localDataPoolMap.emplace(bpxBat::DISCHARGE_CURRENT, &dischargeCurrent);
|
|
localDataPoolMap.emplace(bpxBat::HEATER_CURRENT, &heaterCurrent);
|
|
localDataPoolMap.emplace(bpxBat::BATT_VOLTAGE, &battVolt);
|
|
localDataPoolMap.emplace(bpxBat::REBOOT_COUNTER, &rebootCounter);
|
|
localDataPoolMap.emplace(bpxBat::BOOTCAUSE, &bootCause);
|
|
|
|
localDataPoolMap.emplace(bpxBat::BATTERY_HEATER_MODE, &battheatMode);
|
|
localDataPoolMap.emplace(bpxBat::BATTHEAT_LOW_LIMIT, &battheatLow);
|
|
localDataPoolMap.emplace(bpxBat::BATTHEAT_HIGH_LIMIT, &battheatHigh);
|
|
|
|
poolManager.subscribeForRegularPeriodicPacket(
|
|
subdp::RegularHkPeriodicParams(hkSet.getSid(), enableHkSets, 20.0));
|
|
poolManager.subscribeForRegularPeriodicPacket(
|
|
subdp::RegularHkPeriodicParams(cfgSet.getSid(), false, 30.0));
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void BpxBatteryHandler::setToGoToNormalMode(bool enable) {
|
|
this->goToNormalModeImmediately = enable;
|
|
}
|
|
|
|
void BpxBatteryHandler::setDebugMode(bool enable) { this->debugMode = enable; }
|
|
|
|
void BpxBatteryHandler::performOperationHook() {
|
|
#if OBSW_THREAD_TRACING == 1
|
|
trace::threadTrace(opCounter, "BPX BATT");
|
|
#endif
|
|
}
|