341 lines
11 KiB
C++
341 lines
11 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) :
|
|
DeviceHandlerBase(objectId, comIF, comCookie), maxConfigTableAddress(
|
|
maxConfigTableAddress), maxHkTableAddress(maxHkTableAddress) {
|
|
mode = MODE_NORMAL;
|
|
if (comCookie == NULL) {
|
|
sif::error << "GomspaceDeviceHandler invalid com cookie" << 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(PING): {
|
|
result = generatePingCommand(commandData, commandDataLen);
|
|
if(result != HasReturnvaluesIF::RETURN_OK) {
|
|
return result;
|
|
}
|
|
break;
|
|
}
|
|
case(REBOOT): {
|
|
generateRebootCommand();
|
|
break;
|
|
}
|
|
case(PARAM_SET):{
|
|
result = generateSetParamCommand(commandData, commandDataLen);
|
|
if(result != HasReturnvaluesIF::RETURN_OK){
|
|
return result;
|
|
}
|
|
break;
|
|
}
|
|
case(PARAM_GET):{
|
|
result = generateGetParamCommand(commandData, commandDataLen);
|
|
if(result != HasReturnvaluesIF::RETURN_OK){
|
|
return result;
|
|
}
|
|
break;
|
|
}
|
|
case(GNDWDT_RESET): {
|
|
result = generateResetWatchdogCmd();
|
|
if(result != HasReturnvaluesIF::RETURN_OK){
|
|
return result;
|
|
}
|
|
break;
|
|
}
|
|
default:
|
|
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
void GomspaceDeviceHandler::fillCommandAndReplyMap(){
|
|
this->insertInCommandAndReplyMap(PING, 3);
|
|
this->insertInCommandMap(REBOOT);
|
|
this->insertInCommandAndReplyMap(PARAM_SET, 3);
|
|
this->insertInCommandAndReplyMap(PARAM_GET, 3);
|
|
this->insertInCommandMap(GNDWDT_RESET);
|
|
}
|
|
|
|
ReturnValue_t GomspaceDeviceHandler::scanForReply(const uint8_t *start,
|
|
size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
|
|
switch(rememberCommandId) {
|
|
case(PING):
|
|
*foundId = PING;
|
|
*foundLen = PING_REPLY_SIZE;
|
|
rememberCommandId = NONE;
|
|
break;
|
|
case(PARAM_GET): {
|
|
*foundId = PARAM_GET;
|
|
*foundLen = rememberRequestedSize + CspGetParamReply::GS_HDR_LENGTH;
|
|
rememberCommandId = NONE;
|
|
break;
|
|
}
|
|
case(PARAM_SET): {
|
|
*foundId = PARAM_SET;
|
|
*foundLen = rememberRequestedSize;
|
|
rememberCommandId = 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(PING): {
|
|
SerializeElement<uint32_t> replyTime = *packet;
|
|
handleDeviceTM(&replyTime, id, true);
|
|
break;
|
|
}
|
|
case(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 = CspGetParamReply::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(¶mReply, id, true);
|
|
break;
|
|
}
|
|
case(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;
|
|
}
|
|
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 = 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 + CspGetParamCommand::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 = 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 = 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 = GNDWDT_RESET;
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
uint32_t GomspaceDeviceHandler::getTransitionDelayMs(Mode_t modeFrom,
|
|
Mode_t modeTo) {
|
|
return 0;
|
|
}
|