55 lines
1.6 KiB
C++
55 lines
1.6 KiB
C++
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_
|
|
#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_
|
|
|
|
#include "rwHelpers.h"
|
|
|
|
void rws::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encodedBuffer,
|
|
size_t& encodedLen) {
|
|
encodedBuffer[0] = rws::FRAME_DELIMITER;
|
|
encodedLen = 1;
|
|
for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) {
|
|
if (sourceBuf[sourceIdx] == 0x7E) {
|
|
encodedBuffer[encodedLen++] = 0x7D;
|
|
encodedBuffer[encodedLen++] = 0x5E;
|
|
} else if (sourceBuf[sourceIdx] == 0x7D) {
|
|
encodedBuffer[encodedLen++] = 0x7D;
|
|
encodedBuffer[encodedLen++] = 0x5D;
|
|
} else {
|
|
encodedBuffer[encodedLen++] = sourceBuf[sourceIdx];
|
|
}
|
|
}
|
|
encodedBuffer[encodedLen++] = rws::FRAME_DELIMITER;
|
|
}
|
|
|
|
size_t rws::idToPacketLen(DeviceCommandId_t id) {
|
|
switch (id) {
|
|
case (rws::GET_RW_STATUS): {
|
|
return rws::SIZE_GET_RW_STATUS;
|
|
}
|
|
case (rws::SET_SPEED): {
|
|
return rws::SIZE_SET_SPEED_REPLY;
|
|
}
|
|
case (rws::CLEAR_LAST_RESET_STATUS): {
|
|
return rws::SIZE_CLEAR_RESET_STATUS;
|
|
}
|
|
case (rws::GET_LAST_RESET_STATUS): {
|
|
return rws::SIZE_GET_RESET_STATUS;
|
|
}
|
|
case (rws::GET_TEMPERATURE): {
|
|
return rws::SIZE_GET_TEMPERATURE_REPLY;
|
|
}
|
|
case (rws::GET_TM): {
|
|
return rws::SIZE_GET_TELEMETRY_REPLY;
|
|
}
|
|
case (rws::INIT_RW_CONTROLLER): {
|
|
return rws::SIZE_INIT_RW;
|
|
}
|
|
default: {
|
|
sif::error << "no reply buffer for rw command " << id << std::endl;
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */
|