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

rw handler fixes

Browse Source
This commit is contained in:
Martin Zietz
2021-06-25 15:25:54 +02:00
parent ced90934bb
commit ce3fee33e0
4 changed files with 20 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
mission/devices/RwHandler.cpp
View File

@ -208,7 +208,7 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
return INVALID_SPEED;
}
uint16_t rampTime = *commandData << 8 | *(commandData + 1);
uint16_t rampTime = *(commandData + 4) << 8 | *(commandData + 5);
if (rampTime < 10 || rampTime > 10000) {
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time"
@ -238,8 +238,8 @@ void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDa
commandBuffer[3] = *(commandData + 1);
commandBuffer[4] = *commandData;
/** Ramp time (ms) */
commandBuffer[5] = *(commandData + 1);
commandBuffer[6] = *commandData;
commandBuffer[5] = *(commandData + 5);
commandBuffer[6] = *(commandData + 4);
uint16_t crc = CRC::crc16ccitt(commandBuffer, 7, 0xFFFF);
commandBuffer[7] = static_cast<uint8_t>(crc & 0xFF);
@ -251,10 +251,10 @@ void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDa
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
uint8_t offset = 2;
statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
| *(packet + offset + 1) << 8 | *(packet + offset);
offset += 4;
statusSet.state = *(packet + offset);
offset += 1;
@ -275,7 +275,7 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
void RwHandler::handleTemperatureReply(const uint8_t* packet) {
uint8_t offset = 2;
temperatureSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 1 | *(packet + offset);
| *(packet + offset + 1) << 8 | *(packet + offset);
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
sif::info << "RwHandler::handleTemperatureReply: Temperature: "
<< temperatureSet.temperatureCelcius << " °C" << std::endl;