eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

p60 dock handler completed

Browse Source
This commit is contained in:
Jakob Meier
2020-12-17 13:26:00 +01:00
parent 32bd57d6d1
commit 5fac6424ca
5 changed files with 91 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
mission/devices/P60DockHandler.cpp
View File

@ -34,10 +34,9 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand(
size_t commandDataLen) {
switch(deviceCommand) {
case(PING): {
PingMessageUnpacker pingMessageUnpacker(commandData, commandDataLen);
const uint8_t* pingData = pingMessageUnpacker.getPingData();
uint8_t pingDataSz = pingMessageUnpacker.getPingDataSz();
CspPingCommand cspPingCommand(pingDataSz, pingData);
const uint8_t* pingData = commandData;
uint8_t pingDataSz = commandDataLen;
CspPingCommand cspPingCommand(pingData, pingDataSz);
size_t cspPacketLen = 0;
uint8_t* buffer = cspPacket;
cspPingCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
@ -52,6 +51,16 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand(
rememberCommandId = PING;
break;
}
case(REBOOT): {
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;
break;
}
case(PARAM_SET):{
SetParamMessageUnpacker setParamMessageUnpacker(commandData,
commandDataLen);
@ -112,6 +121,23 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand(
rememberCommandId = PARAM_GET;
break;
}
case(GNDWDT_RESET): {
WatchdogResetCommand watchdogResetCommand;
size_t cspPacketLen = 0;
uint8_t* buffer = cspPacket;
watchdogResetCommand.serialize(&buffer, &cspPacketLen, sizeof(cspPacket),
SerializeIF::Endianness::BIG);
if(cspPacketLen > MAX_PACKET_LEN){
sif::error << "P60DockHandler: Received invalid ground watchdog"
"reset command" << std::endl;
return HasReturnvaluesIF::RETURN_FAILED;
}
rawPacket = cspPacket;
rawPacketLen = cspPacketLen;
rememberRequestedSize = 0; // No bytes will be queried with the ground
// watchdog command.
rememberCommandId = GNDWDT_RESET;
}
default:
break;
}
@ -120,8 +146,10 @@ ReturnValue_t P60DockHandler::buildCommandFromCommand(
void P60DockHandler::fillCommandAndReplyMap(){
this->insertInCommandAndReplyMap(PING, 3);
this->insertInCommandMap(REBOOT);
this->insertInCommandAndReplyMap(PARAM_SET, 3);
this->insertInCommandAndReplyMap(PARAM_GET, 3);
this->insertInCommandMap(GNDWDT_RESET);
}
ReturnValue_t P60DockHandler::scanForReply(const uint8_t *start,
@ -154,8 +182,8 @@ ReturnValue_t P60DockHandler::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) {
switch(id) {
case(PING): {
PingReply pingReply(*packet, *(packet + 1));
handleDeviceTM(&pingReply, id, true, true);
SerializeElement<uint32_t> replyTime = *packet;
handleDeviceTM(&replyTime, id, true);
break;
}
case(PARAM_GET): {