139 lines
3.9 KiB
C++
139 lines
3.9 KiB
C++
#include <mission/devices/P60DockHandler.h>
|
|
#include <mission/devices/devicedefinitions/GomSpacePackets.h>
|
|
|
|
P60DockHandler::P60DockHandler(object_id_t objectId, object_id_t comIF,
|
|
CookieIF * comCookie):DeviceHandlerBase(objectId, comIF, comCookie) {
|
|
mode = MODE_NORMAL;
|
|
if(comCookie == NULL){
|
|
sif::error << "P60DockHandler invalid com cookie" << std::endl;
|
|
}
|
|
}
|
|
|
|
P60DockHandler::~P60DockHandler() {
|
|
}
|
|
|
|
|
|
void P60DockHandler::doStartUp(){
|
|
}
|
|
|
|
void P60DockHandler::doShutDown(){
|
|
|
|
}
|
|
|
|
ReturnValue_t P60DockHandler::buildNormalDeviceCommand(DeviceCommandId_t * id){
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t P60DockHandler::buildTransitionDeviceCommand(
|
|
DeviceCommandId_t * id){
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t P60DockHandler::buildCommandFromCommand(
|
|
DeviceCommandId_t deviceCommand, const uint8_t * commandData,
|
|
size_t commandDataLen) {
|
|
switch(deviceCommand) {
|
|
case(PING): {
|
|
break;
|
|
}
|
|
case(PARAM_SET):{
|
|
break;
|
|
}
|
|
case(PARAM_GET):{
|
|
/* Unpack the received action message */
|
|
GetParamMessageUnpacker getParamMessage(commandData, commandDataLen);
|
|
uint8_t tableId = getParamMessage.getTableId();
|
|
uint16_t address = EndianConverter::convertLittleEndian<uint16_t>(
|
|
getParamMessage.getAddress());
|
|
uint16_t length = EndianConverter::convertLittleEndian<uint16_t>(
|
|
sizeof(address));
|
|
uint16_t checksum = GOMSPACE::IGNORE_CHECKSUM;
|
|
uint16_t seq = 0;
|
|
uint16_t total = 0;
|
|
uint16_t querySize = getParamMessage.getQuerySize()
|
|
+ CspGetParamCommand::GS_HDR_LENGTH;
|
|
/* Generate the CSP command to send to the P60 Dock */
|
|
CspGetParamCommand getParamCmd(querySize, PARAM_GET, tableId, length,
|
|
checksum, seq, total, address);
|
|
size_t cspPacketLen = 0;
|
|
uint8_t* buffer = cspPacket;
|
|
getParamCmd.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
|
|
SerializeIF::Endianness::BIG);
|
|
if(cspPacketLen > MAX_PACKET_LEN){
|
|
sif::error << "P60DockHandler: Received invalid get parameter "
|
|
"command" << std::endl;
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
rawPacket = cspPacket;
|
|
rawPacketLen = cspPacketLen;
|
|
rememberRequestedSize = querySize;
|
|
rememberCommandId = PARAM_GET;
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void P60DockHandler::fillCommandAndReplyMap(){
|
|
this->insertInCommandAndReplyMap(PING, 3);
|
|
this->insertInCommandAndReplyMap(PARAM_SET, 3);
|
|
this->insertInCommandAndReplyMap(PARAM_GET, 3);
|
|
}
|
|
|
|
ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start,
|
|
size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) {
|
|
switch(rememberCommandId) {
|
|
case(PING):
|
|
*foundId = PING;
|
|
*foundLen = rememberRequestedSize;
|
|
rememberCommandId = NONE;
|
|
break;
|
|
case(PARAM_GET): {
|
|
*foundId = PARAM_GET;
|
|
*foundLen = rememberRequestedSize + CspGetParamReply::GS_HDR_LENGTH;
|
|
rememberCommandId = NONE;
|
|
break;
|
|
}
|
|
default:
|
|
return IGNORE_REPLY_DATA;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t P60DockHandler::interpretDeviceReply(DeviceCommandId_t id,
|
|
const uint8_t *packet) {
|
|
switch(id) {
|
|
case(PING): {
|
|
handleDeviceTM((SerializeIF*)packet, id, true, true);
|
|
break;
|
|
}
|
|
case(PARAM_GET): {
|
|
uint16_t payloadLength = *(packet + 2);
|
|
uint8_t tempPayloadBuffer[payloadLength];
|
|
CspGetParamReply cspGetParamReply(tempPayloadBuffer, payloadLength);
|
|
uint8_t action = cspGetParamReply.getAction();
|
|
uint8_t tableId = cspGetParamReply.getTableId();
|
|
uint16_t length = cspGetParamReply.getLength();
|
|
uint16_t address = cspGetParamReply.getAddress();
|
|
size_t size = CspGetParamReply::GS_HDR_LENGTH + payloadLength;
|
|
cspGetParamReply.deSerialize(&packet, &size,
|
|
SerializeIF::Endianness::LITTLE);
|
|
ParamReply paramReply(action, tableId, address, length, tempPayloadBuffer,
|
|
payloadLength);
|
|
handleDeviceTM(¶mReply, id, true, true);
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
void P60DockHandler::setNormalDatapoolEntriesInvalid(){
|
|
|
|
}
|
|
|
|
|