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

rwHandler rwSpeedActuationSet handling

Browse Source
This commit is contained in:
Marius Eggert 2023-02-10 13:16:50 +01:00
parent 33684f2ef7
commit 08df102f36
2 changed files with 49 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
mission/devices/RwHandler.cpp
View File

@ -13,7 +13,8 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki
enableGpio(enableGpio),
statusSet(this),
lastResetStatusSet(this),
tmDataset(this) {
tmDataset(this),
rwSpeedActuationSet(*this) {
if (comCookie == nullptr) {
sif::error << "RwHandler: Invalid com cookie" << std::endl;
}
@ -97,16 +98,36 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
return returnvalue::OK;
}
case (RwDefinitions::SET_SPEED): {
if (commandDataLen != 6) {
if (commandData != nullptr && commandDataLen != 6) {
sif::error << "RwHandler::buildCommandFromCommand: Received set speed command with"
<< " invalid length" << std::endl;
return SET_SPEED_COMMAND_INVALID_LENGTH;
}
result = checkSpeedAndRampTime(commandData, commandDataLen);
// Commands override anything which was set in the software
if (commandData != nullptr) {
rwSpeedActuationSet.setValidityBufferGeneration(false);
result = rwSpeedActuationSet.deSerialize(&commandData, &commandDataLen,
SerializeIF::Endianness::NETWORK);
rwSpeedActuationSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
prepareSetSpeedCmd(commandData, commandDataLen);
} else {
// Read set rw speed value from local pool
PoolReadGuard pg(&rwSpeedActuationSet);
}
if (ACTUATION_WIRETAPPING) {
int32_t speed;
uint16_t rampTime;
rwSpeedActuationSet.getRwSpeed(speed, rampTime);
sif::debug << "Actuating RW with speed = " << speed << " and rampTime = " << rampTime
<< std::endl;
}
result = checkSpeedAndRampTime();
if (result != returnvalue::OK) {
return result;
}
result = prepareSetSpeedCmd();
return result;
}
case (RwDefinitions::GET_TEMPERATURE): {
@ -298,17 +319,15 @@ void RwHandler::prepareSimpleCommand(DeviceCommandId_t id) {
rawPacketLen = 3;
}
ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen) {
int32_t speed =
*commandData << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3);
ReturnValue_t RwHandler::checkSpeedAndRampTime() {
int32_t speed = 0;
uint16_t rampTime = 0;
rwSpeedActuationSet.getRwSpeed(speed, rampTime);
if ((speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) && (speed != 0)) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
return INVALID_SPEED;
}
uint16_t rampTime = (*(commandData + 4) << 8) | *(commandData + 5);
if (rampTime < 10 || rampTime > 20000) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time" << std::endl;
return INVALID_RAMP_TIME;
@ -317,23 +336,24 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
return returnvalue::OK;
}
void RwHandler::prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen) {
ReturnValue_t RwHandler::prepareSetSpeedCmd() {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::SET_SPEED);
/** Speed (0.1 RPM) */
commandBuffer[1] = *(commandData + 3);
commandBuffer[2] = *(commandData + 2);
commandBuffer[3] = *(commandData + 1);
commandBuffer[4] = *commandData;
/** Ramp time (ms) */
commandBuffer[5] = *(commandData + 5);
commandBuffer[6] = *(commandData + 4);
uint8_t* serPtr = commandBuffer + 1;
size_t serSize = 1;
rwSpeedActuationSet.setValidityBufferGeneration(false);
ReturnValue_t result = rwSpeedActuationSet.serialize(&serPtr, &serSize, sizeof(commandBuffer),
SerializeIF::Endianness::LITTLE);
rwSpeedActuationSet.setValidityBufferGeneration(true);
if (result != returnvalue::OK) {
return result;
}
uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
commandBuffer[8] = static_cast<uint8_t>(crc >> 8 & 0xFF);
commandBuffer[8] = static_cast<uint8_t>((crc >> 8) & 0xFF);
rawPacket = commandBuffer;
rawPacketLen = 9;
return result;
}
void RwHandler::handleResetStatusReply(const uint8_t* packet) {

12
mission/devices/RwHandler.h
View File

@ -9,6 +9,8 @@
#include "events/subsystemIdRanges.h"
#include "returnvalues/classIds.h"
static constexpr bool ACTUATION_WIRETAPPING = false;
class GpioIF;
/**
@ -93,6 +95,7 @@ class RwHandler : public DeviceHandlerBase {
RwDefinitions::StatusSet statusSet;
RwDefinitions::LastResetSatus lastResetStatusSet;
RwDefinitions::TmDataset tmDataset;
RwDefinitions::RwSpeedActuationSet rwSpeedActuationSet;
uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
@ -117,13 +120,14 @@ class RwHandler : public DeviceHandlerBase {
* range.
* @return returnvalue::OK if successful, otherwise error code.
*/
ReturnValue_t checkSpeedAndRampTime(const uint8_t* commandData, size_t commandDataLen);
ReturnValue_t checkSpeedAndRampTime();
/**
* @brief This function prepares the set speed command from the commandData received with
* an action message.
* @brief This function prepares the set speed command from the dataSet received with
* an action message or set in the software.
* @return returnvalue::OK if successful, otherwise error code.
*/
void prepareSetSpeedCmd(const uint8_t* commandData, size_t commandDataLen);
ReturnValue_t prepareSetSpeedCmd();
/**
* @brief This function writes the last reset status retrieved with the get last reset status