get temperature and status of reaction wheel

This commit is contained in:
Jakob Meier 2021-06-24 17:32:42 +02:00
parent d05b1913ab
commit 31f9265338
5 changed files with 89 additions and 97 deletions

View File

@ -62,14 +62,7 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
sif::error << "rwSpiCallback: Write failed!" << std::endl;
if(gpioId != gpio::NO_GPIO) {
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
}
}
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to unlock mutex" << std::endl;;
}
closeSpi(gpioId, gpioIF, mutex);
return RwHandler::SPI_WRITE_FAILURE;
}
@ -94,14 +87,7 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s
}
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
sif::error << "rwSpiCallback: Write failed!" << std::endl;
if(gpioId != gpio::NO_GPIO) {
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
}
}
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to unlock mutex";
}
closeSpi(gpioId, gpioIF, mutex);
return RwHandler::SPI_WRITE_FAILURE;
}
idx++;
@ -113,28 +99,14 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s
if (write(fileDescriptor, writeBuffer, writeSize) != static_cast<ssize_t>(writeSize)) {
sif::error << "rwSpiCallback: Write failed!" << std::endl;
if(gpioId != gpio::NO_GPIO) {
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
}
}
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to unlock mutex";
}
closeSpi(gpioId, gpioIF, mutex);
return RwHandler::SPI_WRITE_FAILURE;
}
uint8_t* rxBuf = nullptr;
result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf);
if(result != HasReturnvaluesIF::RETURN_OK) {
if(gpioId != gpio::NO_GPIO) {
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
}
}
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to unlock mutex";
}
closeSpi(gpioId, gpioIF, mutex);
return result;
}
@ -143,56 +115,40 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s
/** There must be a delay of 20 ms after sending the command */
usleep(RwDefinitions::SPI_REPLY_DELAY);
/** Receiving reply data */
uint8_t byteRead = 0;
/** Reading the reply frame */
// if(read(fileDescriptor, &byteRead, 1) != 1) {
// if(gpioId != gpio::NO_GPIO) {
// if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
// sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
// }
// }
// if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
// sif::error << "rwSpiCallback: Failed to unlock mutex";
// }
// sif::error << "rwSpiCallback: Failed to read first byte of reply frame" << std::endl;
// return RwHandler::SPI_READ_FAILURE;
// }
uint8_t readbuffer[10];
if(read(fileDescriptor, readbuffer, 10) != 10) {
sif::error << "Failed to read all bytes" << std::endl;;
}
/** First byte must be the start sign 0x7E */
if (byteRead != 0x7E) {
sif::error << "rwSpiCallback: First byte of reply is not 0x7E" << std::endl;
if(gpioId != gpio::NO_GPIO) {
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
}
}
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to unlock mutex";
}
return RwHandler::MISSING_START_SIGN;
}
/**
* The reaction wheel responds with empty frames while preparing the reply data.
* However, receiving more than 5 empty frames will be interpreted as an error.
*/
uint8_t byteRead = 0;
for (int idx = 0; idx < 10; idx++) {
sif::error << "rwSpiCallback: Empty frame timeout";
return RwHandler::NO_REPLY;
if(read(fileDescriptor, &byteRead, 1) != 1) {
sif::error << "rwSpiCallback: Read failed" << std::endl;
closeSpi(gpioId, gpioIF, mutex);
return RwHandler::SPI_READ_FAILURE;
}
if (byteRead != 0x7E) {
break;
}
if (idx == 9) {
sif::error << "rwSpiCallback: Empty frame timeout" << std::endl;
closeSpi(gpioId, gpioIF, mutex);
return RwHandler::NO_REPLY;
}
}
size_t decodedFrameLen = 0;
for (; decodedFrameLen < replyBufferSize; decodedFrameLen++) {
byteRead = 0;
if(read(fileDescriptor, &byteRead, 1) != 1) {
sif::error << "rwSpiCallback: Read failed" << std::endl;
return RwHandler::SPI_READ_FAILURE;
while(decodedFrameLen < replyBufferSize) {
/** First byte already read in */
if (decodedFrameLen != 0) {
byteRead = 0;
if(read(fileDescriptor, &byteRead, 1) != 1) {
sif::error << "rwSpiCallback: Read failed" << std::endl;
result = RwHandler::SPI_READ_FAILURE;
break;
}
}
if (byteRead == 0x7E) {
@ -217,6 +173,7 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s
}
else {
sif::error << "rwSpiCallback: Invalid substitute" << std::endl;
closeSpi(gpioId, gpioIF, mutex);
result = RwHandler::INVALID_SUBSTITUTE;
break;
}
@ -250,16 +207,18 @@ ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *s
cookie->assignTransferSize(decodedFrameLen);
if(gpioId != gpio::NO_GPIO) {
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to pull chip select high" << std::endl;
}
}
closeSpi(gpioId, gpioIF, mutex);
if(gpioId != gpio::NO_GPIO) {
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
sif::error << "rwSpiCallback: Failed to unlock mutex" << std::endl;
}
}
return result;
}
void closeSpi (gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex) {
if(gpioId != gpio::NO_GPIO) {
if (gpioIF->pullHigh(gpioId) != HasReturnvaluesIF::RETURN_OK) {
sif::error << "closeSpi: Failed to pull chip select high" << std::endl;
}
}
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
sif::error << "closeSpi: Failed to unlock mutex" << std::endl;;
}
}

View File

@ -18,4 +18,13 @@
ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData,
size_t sendLen, void* args);
/**
* @brief This function closes a spi session. Pulls the chip select to high an releases the
* mutex.
* @param gpioId Gpio ID of chip select
* @param gpioIF Pointer to gpio interface to drive the chip select
* @param mutex The spi mutex
*/
void closeSpi(gpioId_t gpioId, GpioIF* gpioIF, MutexIF* mutex);
#endif /* BSP_Q7S_RW_SPI_CALLBACK_H_ */

@ -1 +1 @@
Subproject commit 8fe5d0afa0857025d9dee3c6f266c2503a22c517
Subproject commit 2e243e3b294870b0c93f85c6b8ff0c74f4f9cce7

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;

View File

@ -18,6 +18,15 @@ enum PoolIds: lp_id_t {
CLC_MODE
};
enum States: uint8_t {
ERROR,
IDLE,
COASTING,
RUNNING_SPEED_STABLE,
RUNNING_SPEED_CHANGING
};
static const DeviceCommandId_t RESET_MCU = 1;
static const DeviceCommandId_t GET_RW_STATUS = 4;
static const DeviceCommandId_t SET_SPEED = 6;
static const DeviceCommandId_t GET_TEMPERATURE = 8;