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

get temperature and status of reaction wheel

Browse Source
This commit is contained in:
Jakob Meier
2021-06-24 17:32:42 +02:00
parent 8284ebec9f
commit b5fdeb93bb
5 changed files with 89 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
mission/devices/RwHandler.cpp
View File

@ -40,9 +40,11 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
switch (communicationStep) {
case CommunicationStep::READ_TEMPERATURE:
*id = RwDefinitions::GET_TEMPERATURE;
communicationStep = CommunicationStep::GET_RW_SATUS;
break;
case CommunicationStep::GET_RW_SATUS:
*id = RwDefinitions::GET_RW_STATUS;
communicationStep = CommunicationStep::READ_TEMPERATURE;
break;
default:
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid communication step"
@ -61,6 +63,12 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
ReturnValue_t result = RETURN_OK;
switch (deviceCommand) {
case (RwDefinitions::RESET_MCU): {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::RESET_MCU);
rawPacket = commandBuffer;
rawPacketLen = 1;
return RETURN_OK;
}
case (RwDefinitions::GET_RW_STATUS): {
prepareGetStatusCmd(commandData, commandDataLen);
return RETURN_OK;
@ -89,6 +97,9 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
}
void RwHandler::fillCommandAndReplyMap() {
this->insertInCommandMap(RwDefinitions::RESET_MCU);
this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet,
RwDefinitions::SIZE_GET_RW_STATUS);
this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, &temperatureSet,
RwDefinitions::SIZE_GET_TEMPERATURE_REPLY);
this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr,
@ -98,7 +109,7 @@ void RwHandler::fillCommandAndReplyMap() {
ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize,
DeviceCommandId_t *foundId, size_t *foundLen) {
switch (*(start + 1)) {
switch (*(start)) {
case (static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS)): {
*foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
*foundId = RwDefinitions::GET_RW_STATUS;
@ -129,14 +140,15 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
/** Check result code */
if (*(packet + 1) != 0) {
if (*(packet + 1) == RwDefinitions::ERROR) {
return EXECUTION_FAILED;
}
/** Received in little endian byte order */
uint16_t replyCrc = *(packet + sizeOfReply - 1) << 8 | *(packet + sizeOfReply - 2) ;
if (CRC::crc16ccitt(packet, sizeOfReply, 0xFFFF) != replyCrc) {
if (CRC::crc16ccitt(packet, sizeOfReply - 2, 0xFFFF) != replyCrc) {
sif::error << "RwHandler::interpretDeviceReply: cRC error" << std::endl;
return CRC_ERROR;
}
@ -237,10 +249,12 @@ void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDa
}
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
uint8_t offset = 0;
statusSet.currSpeed = *(packet + 3) << 24 | *(packet + 2) << 16 | *(packet + 1) << 1 | *packet;
uint8_t offset = 2;
statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
offset += 4;
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 1 | *(packet + offset);
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
offset += 4;
statusSet.state = *(packet + offset);
offset += 1;
@ -251,16 +265,17 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
<< " * 0.1 RPM" << std::endl;
sif::info << "RwHandler::handleGetRwStatusReply: Reference speed is: "
<< statusSet.referenceSpeed << " * 0.1 RPM" << std::endl;
sif::info << "RwHandler::handleGetRwStatusReply: State is: " << statusSet.state
<< " * 0.1 RPM" << std::endl;
sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: " << statusSet.clcMode
<< " * 0.1 RPM" << std::endl;
sif::info << "RwHandler::handleGetRwStatusReply: State is: "
<< (unsigned int) statusSet.state.value << std::endl;
sif::info << "RwHandler::handleGetRwStatusReply: clc mode is: "
<< (unsigned int) statusSet.clcMode.value << std::endl;
#endif
}
void RwHandler::handleTemperatureReply(const uint8_t* packet) {
temperatureSet.temperatureCelcius = *(packet + 3) << 24 | *(packet + 2) << 16
| *(packet + 1) << 1 | *packet;
uint8_t offset = 2;
temperatureSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
sif::info << "RwHandler::handleTemperatureReply: Temperature: "
<< temperatureSet.temperatureCelcius << " °C" << std::endl;