eive-obsw/mission/devices/GomspaceDeviceHandler.cpp
Robin Mueller cbb7fdfa99
Some checks are pending
EIVE/eive-obsw/pipeline/head Build started...
EIVE/eive-obsw/pipeline/pr-develop This commit looks good
p60 print command
2021-09-20 11:47:19 +02:00

409 lines
15 KiB
C++

#include <mission/devices/GomspaceDeviceHandler.h>
#include <mission/devices/devicedefinitions/GomSpacePackets.h>
GomspaceDeviceHandler::GomspaceDeviceHandler(object_id_t objectId, object_id_t comIF,
CookieIF * comCookie, uint16_t maxConfigTableAddress, uint16_t maxHkTableAddress,
uint16_t hkTableReplySize, LocalPoolDataSetBase* hkTableDataset) :
DeviceHandlerBase(objectId, comIF, comCookie), maxConfigTableAddress(maxConfigTableAddress),
maxHkTableAddress(maxHkTableAddress), hkTableReplySize(hkTableReplySize),
hkTableDataset(hkTableDataset) {
if (comCookie == nullptr) {
sif::error << "GomspaceDeviceHandler::GomspaceDeviceHandler: Invalid com cookie"
<< std::endl;
}
if (hkTableDataset == nullptr) {
sif::error << "GomspaceDeviceHandler::GomspaceDeviceHandler: Invalid hk table data set"
<< std::endl;
}
}
GomspaceDeviceHandler::~GomspaceDeviceHandler() {
}
void GomspaceDeviceHandler::doStartUp(){
}
void GomspaceDeviceHandler::doShutDown(){
}
ReturnValue_t GomspaceDeviceHandler::buildNormalDeviceCommand(
DeviceCommandId_t * id) {
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GomspaceDeviceHandler::buildTransitionDeviceCommand(
DeviceCommandId_t * id){
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GomspaceDeviceHandler::buildCommandFromCommand(
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
size_t commandDataLen) {
ReturnValue_t result;
switch(deviceCommand) {
case(GOMSPACE::PING): {
result = generatePingCommand(commandData, commandDataLen);
if(result != HasReturnvaluesIF::RETURN_OK) {
return result;
}
break;
}
case(GOMSPACE::REBOOT): {
generateRebootCommand();
break;
}
case(GOMSPACE::PARAM_SET):{
result = generateSetParamCommand(commandData, commandDataLen);
if(result != HasReturnvaluesIF::RETURN_OK){
return result;
}
break;
}
case(GOMSPACE::PARAM_GET):{
result = generateGetParamCommand(commandData, commandDataLen);
if(result != HasReturnvaluesIF::RETURN_OK){
return result;
}
break;
}
case(GOMSPACE::GNDWDT_RESET): {
result = generateResetWatchdogCmd();
if(result != HasReturnvaluesIF::RETURN_OK){
return result;
}
break;
}
case(GOMSPACE::PRINT_SWITCH_V_I): {
result = printStatus(deviceCommand);
break;
}
case(GOMSPACE::REQUEST_HK_TABLE): {
result = generateRequestFullHkTableCmd(hkTableReplySize);
if(result != HasReturnvaluesIF::RETURN_OK){
return result;
}
break;
}
default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
}
return HasReturnvaluesIF::RETURN_OK;
}
void GomspaceDeviceHandler::fillCommandAndReplyMap(){
this->insertInCommandAndReplyMap(GOMSPACE::PING, 3);
this->insertInCommandMap(GOMSPACE::REBOOT);
this->insertInCommandAndReplyMap(GOMSPACE::PARAM_SET, 3);
this->insertInCommandAndReplyMap(GOMSPACE::PARAM_GET, 3);
this->insertInCommandAndReplyMap(GOMSPACE::REQUEST_HK_TABLE, 3);
this->insertInCommandMap(GOMSPACE::GNDWDT_RESET);
this->insertInCommandMap(GOMSPACE::PRINT_SWITCH_V_I);
}
ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t *start,
size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
switch(rememberCommandId) {
case(GOMSPACE::PING):
*foundId = GOMSPACE::PING;
*foundLen = PING_REPLY_SIZE;
rememberCommandId = GOMSPACE::NONE;
break;
case(GOMSPACE::PARAM_GET): {
*foundId = GOMSPACE::PARAM_GET;
*foundLen = rememberRequestedSize + GOMSPACE::GS_HDR_LENGTH;
rememberCommandId = GOMSPACE::NONE;
break;
}
case(GOMSPACE::PARAM_SET): {
*foundId = GOMSPACE::PARAM_SET;
*foundLen = rememberRequestedSize;
rememberCommandId = GOMSPACE::NONE;
break;
}
case(GOMSPACE::REQUEST_HK_TABLE): {
*foundId = GOMSPACE::REQUEST_HK_TABLE;
*foundLen = rememberRequestedSize + GOMSPACE::GS_HDR_LENGTH;
rememberCommandId = GOMSPACE::NONE;
break;
}
default:
return IGNORE_REPLY_DATA;
}
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GomspaceDeviceHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
switch(id) {
case(GOMSPACE::PING): {
SerializeElement<uint32_t> replyTime = *packet;
handleDeviceTM(&replyTime, id, true);
break;
}
case(GOMSPACE::PARAM_GET): {
// -2 to subtract address size from gomspace parameter reply packet
uint16_t payloadLength = (*(packet + 2) << 8 | *(packet + 3)) - 2;
if(payloadLength > sizeof(uint32_t)){
sif::error << "GomspaceDeviceHandler: PARAM_GET: Invalid payload "
<< "size in reply" << std::endl;
return INVALID_PAYLOAD_SIZE;
}
uint8_t tempPayloadBuffer[payloadLength];
/* Extract information from received data */
CspGetParamReply cspGetParamReply(tempPayloadBuffer, payloadLength);
size_t size = GOMSPACE::GS_HDR_LENGTH + payloadLength;
ReturnValue_t result = cspGetParamReply.deSerialize(&packet, &size,
SerializeIF::Endianness::BIG);
if(result != HasReturnvaluesIF::RETURN_OK){
sif::error << "GomspaceDeviceHandler: Failed to deserialize get parameter"
<< "reply" << std::endl;
return result;
}
uint8_t action = cspGetParamReply.getAction();
uint8_t tableId = cspGetParamReply.getTableId();
uint16_t address = cspGetParamReply.getAddress();
/* Pack relevant information into a tm packet */
ParamReply paramReply(action, tableId, address, payloadLength,
tempPayloadBuffer);
handleDeviceTM(&paramReply, id, true);
break;
}
case(GOMSPACE::PARAM_SET): {
/* When setting a parameter, the p60dock sends back the state of the
* operation */
if(*packet != PARAM_SET_OK){
return HasReturnvaluesIF::RETURN_FAILED;
}
break;
}
case(GOMSPACE::REQUEST_HK_TABLE): {
letChildHandleHkReply(id, packet);
break;
}
default:
break;
}
return HasReturnvaluesIF::RETURN_OK;
}
void GomspaceDeviceHandler::setNormalDatapoolEntriesInvalid(){
}
ReturnValue_t GomspaceDeviceHandler::generateSetParamCommand(
const uint8_t * commandData, size_t commandDataLen) {
SetParamMessageUnpacker setParamMessageUnpacker;
ReturnValue_t result = setParamMessageUnpacker.deSerialize(&commandData,
&commandDataLen, SerializeIF::Endianness::BIG);
if(result != HasReturnvaluesIF::RETURN_OK){
sif::error << "GomspaceDeviceHandler: Failed to deserialize set parameter "
"message" << std::endl;
return result;
}
/* Get and check address */
uint16_t address = setParamMessageUnpacker.getAddress();
if(address > maxConfigTableAddress){
sif::error << "GomspaceDeviceHandler: Invalid address for set parameter "
<< "action" << std::endl;
return INVALID_ADDRESS;
}
uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM;
uint16_t seq = 0;
uint16_t total = 0;
/* CSP reply only contains the transaction state */
uint16_t querySize = 1;
const uint8_t* parameterPtr = setParamMessageUnpacker.getParameter();
uint8_t parameterSize = setParamMessageUnpacker.getParameterSize();
uint16_t payloadlength = sizeof(address) + parameterSize;
/* Generate command for CspComIF */
CspSetParamCommand setParamCmd(querySize, payloadlength, checksum, seq,
total, address, parameterPtr, parameterSize);
size_t cspPacketLen = 0;
uint8_t* buffer = cspPacket;
result = setParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
SerializeIF::Endianness::BIG);
if(result != HasReturnvaluesIF::RETURN_OK){
sif::error << "GomspaceDeviceHandler: Failed to serialize command for "
<< "CspComIF" << std::endl;
return result;
}
if(cspPacketLen > MAX_PACKET_LEN){
sif::error << "GomspaceDeviceHandler: Invalid length of set parameter "
"command" << std::endl;
return PACKET_TOO_LONG;
}
rawPacket = cspPacket;
rawPacketLen = cspPacketLen;
rememberRequestedSize = querySize;
rememberCommandId = GOMSPACE::PARAM_SET;
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GomspaceDeviceHandler::generateGetParamCommand(
const uint8_t * commandData, size_t commandDataLen){
ReturnValue_t result;
/* Unpack the received action message */
GetParamMessageUnpacker getParamMessage;
result = getParamMessage.deSerialize(&commandData, &commandDataLen,
SerializeIF::Endianness::BIG);
if(result != HasReturnvaluesIF::RETURN_OK) {
sif::error << "Failed to deserialize message to extract information "
"from get parameter message" << std::endl;
return result;
}
/* Get an check table id to read from */
uint8_t tableId = getParamMessage.getTableId();
if(tableId != CONFIG_TABLE_ID && tableId != HK_TABLE_ID){
sif::error << "GomspaceDeviceHandler: Invalid table id in get parameter"
" message" << std::endl;
return INVALID_TABLE_ID;
}
/* Get and check address */
uint16_t address = getParamMessage.getAddress();
if(address > maxHkTableAddress && tableId == HK_TABLE_ID){
sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from "
<< "housekeeping table" << std::endl;
return INVALID_ADDRESS;
}
if(address > maxConfigTableAddress && tableId == CONFIG_TABLE_ID){
sif::error << "GomspaceDeviceHandler: Invalid address to get parameter from "
<< "configuration table" << std::endl;
return INVALID_ADDRESS;
}
uint16_t length = sizeof(address);
uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM;
uint16_t seq = 0;
uint16_t total = 0;
uint8_t parameterSize = getParamMessage.getParameterSize();
if(parameterSize > sizeof(uint32_t)) {
sif::error << "GomspaceDeviceHandler: GET_PARAM: Invalid parameter "
<< "size" << std::endl;
return INVALID_PARAM_SIZE;
}
uint16_t querySize = parameterSize + GOMSPACE::GS_HDR_LENGTH;
/* Generate the CSP command to send to the P60 Dock */
CspGetParamCommand getParamCmd(querySize, tableId, length,
checksum, seq, total, address);
size_t cspPacketLen = 0;
uint8_t* buffer = cspPacket;
result = getParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
SerializeIF::Endianness::BIG);
if(result != HasReturnvaluesIF::RETURN_OK){
sif::error << "GomspaceDeviceHandler: Failed to serialize command to "
<< "get parameter" << std::endl;
}
if(cspPacketLen > MAX_PACKET_LEN){
sif::error << "GomspaceDeviceHandler: Received invalid get parameter "
"command" << std::endl;
return PACKET_TOO_LONG;
}
rawPacket = cspPacket;
rawPacketLen = cspPacketLen;
rememberRequestedSize = querySize;
rememberCommandId = GOMSPACE::PARAM_GET;
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GomspaceDeviceHandler::generatePingCommand(
const uint8_t * commandData, size_t commandDataLen) {
CspPingCommand cspPingCommand(commandData, commandDataLen);
size_t cspPacketLen = 0;
uint8_t* buffer = cspPacket;
ReturnValue_t result = cspPingCommand.serialize(&buffer, &cspPacketLen,
sizeof(cspPacket),
SerializeIF::Endianness::BIG);
if(result != HasReturnvaluesIF::RETURN_OK){
sif::error << "GomspaceDeviceHandler: Failed to serialize ping command"
<< std::endl;
return result;
}
if(cspPacketLen > MAX_PACKET_LEN){
sif::error << "GomspaceDeviceHandler: Received invalid ping message"
<< std::endl;
return PACKET_TOO_LONG;
}
rawPacket = cspPacket;
rawPacketLen = cspPacketLen;
rememberCommandId = GOMSPACE::PING;
return HasReturnvaluesIF::RETURN_OK;
}
void GomspaceDeviceHandler::generateRebootCommand(){
uint8_t cspPort = GOMSPACE::REBOOT_PORT;
uint16_t querySize = 0;
*cspPacket = GOMSPACE::REBOOT_PORT;
*(cspPacket + 1) = querySize;
size_t cspPacketLen = sizeof(cspPort) + sizeof(cspPacketLen);
rawPacket = cspPacket;
rawPacketLen = cspPacketLen;
}
ReturnValue_t GomspaceDeviceHandler::generateResetWatchdogCmd(){
WatchdogResetCommand watchdogResetCommand;
size_t cspPacketLen = 0;
uint8_t* buffer = cspPacket;
ReturnValue_t result = watchdogResetCommand.serialize(&buffer,
&cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG);
if(result != HasReturnvaluesIF::RETURN_OK){
sif::error << "GomspaceDeviceHandler: Failed to serialize watchdog reset "
<< "command" << std::endl;
return result;
}
rawPacket = cspPacket;
rawPacketLen = cspPacketLen;
rememberRequestedSize = 0; // No bytes will be queried with the ground
// watchdog command.
rememberCommandId = GOMSPACE::GNDWDT_RESET;
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GomspaceDeviceHandler::generateRequestFullHkTableCmd(uint16_t hkTableReplySize) {
uint16_t querySize = hkTableReplySize;
uint8_t tableId = HK_TABLE_ID;
RequestFullTableCommand requestFullTableCommand(querySize, tableId);
size_t cspPacketLen = 0;
uint8_t* buffer = cspPacket;
ReturnValue_t result = requestFullTableCommand.serialize(&buffer,
&cspPacketLen, sizeof(cspPacket), SerializeIF::Endianness::BIG);
if(result != HasReturnvaluesIF::RETURN_OK){
sif::error << "GomspaceDeviceHandler::generateRequestFullHkTableCmd Failed to serialize "
"full table request command " << std::endl;
return result;
}
rawPacket = cspPacket;
rawPacketLen = cspPacketLen;
rememberRequestedSize = querySize;
rememberCommandId = GOMSPACE::REQUEST_HK_TABLE;
return RETURN_OK;
}
uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom,
Mode_t modeTo) {
return 0;
}
LocalPoolDataSetBase* GomspaceDeviceHandler::getDataSetHandle(sid_t sid) {
if(sid == hkTableDataset->getSid()) {
return hkTableDataset;
}
else {
return nullptr;
}
}
void GomspaceDeviceHandler::setModeNormal() {
mode = MODE_NORMAL;
}
ReturnValue_t GomspaceDeviceHandler::printStatus(DeviceCommandId_t cmd) {
sif::info << "No printHkTable implementation given.." << std::endl;
return HasReturnvaluesIF::RETURN_OK;
}