35 lines
1.4 KiB
C++
35 lines
1.4 KiB
C++
#include <fsfw/globalfunctions/CRC.h>
|
|
#include <mission/payload/scexHelpers.h>
|
|
|
|
#include <cstring>
|
|
|
|
uint8_t scex::createCmdByte(Cmds cmd, bool tempCheck) {
|
|
return (IDLE_BIT_0_DEF_STATE << 7) | (IDLE_BIT_1_DEF_STATE << 6) | (cmd << 1) | tempCheck;
|
|
}
|
|
|
|
ReturnValue_t scex::prepareScexCmd(Cmds cmd, std::pair<uint8_t*, size_t> cmdBufPair, size_t& cmdLen,
|
|
std::pair<const uint8_t*, size_t> usrDataPair, bool tempCheck) {
|
|
using namespace scex;
|
|
uint8_t* cmdBuf = cmdBufPair.first;
|
|
const uint8_t* userData = usrDataPair.first;
|
|
// Send command
|
|
if (cmdBuf == nullptr or (cmdBufPair.second < usrDataPair.second + HEADER_LEN + CRC_LEN) or
|
|
(usrDataPair.second > 0 and userData == nullptr)) {
|
|
cmdLen = 0;
|
|
return returnvalue::FAILED;
|
|
}
|
|
cmdBuf[0] = createCmdByte(cmd, tempCheck);
|
|
// These two fields are the packet counter and the total packet count. Those are 1 and 1 for each
|
|
// telecommand so far
|
|
cmdBuf[1] = 1;
|
|
cmdBuf[2] = 1;
|
|
cmdBuf[3] = (usrDataPair.second >> 8) & 0xff;
|
|
cmdBuf[4] = usrDataPair.second & 0xff;
|
|
std::memcpy(cmdBuf + HEADER_LEN, userData, usrDataPair.second);
|
|
uint16_t crc = CRC::crc16ccitt(cmdBuf, usrDataPair.second + HEADER_LEN);
|
|
cmdBuf[usrDataPair.second + HEADER_LEN] = (crc >> 8) & 0xff;
|
|
cmdBuf[usrDataPair.second + HEADER_LEN + 1] = crc & 0xff;
|
|
cmdLen = usrDataPair.second + HEADER_LEN + CRC_LEN;
|
|
return returnvalue::OK;
|
|
}
|