RwHandler first commands
This commit is contained in:
parent
0913442804
commit
33a55d7114
@ -9,3 +9,4 @@ add_subdirectory(comIF)
|
||||
add_subdirectory(boardtest)
|
||||
add_subdirectory(gpio)
|
||||
add_subdirectory(core)
|
||||
add_subdirectory(spiCallbacks)
|
||||
|
||||
@ -9,6 +9,7 @@
|
||||
|
||||
#include "bsp_q7s/gpio/gpioCallbacks.h"
|
||||
#include "bsp_q7s/core/CoreController.h"
|
||||
#include "bsp_q7s/spiCallbacks/rwSpiCallback.h"
|
||||
|
||||
#include <linux/devices/HeaterHandler.h>
|
||||
#include <linux/devices/SolarArrayDeploymentHandler.h>
|
||||
@ -31,11 +32,13 @@
|
||||
#include <mission/devices/MGMHandlerRM3100.h>
|
||||
#include <mission/devices/PlocHandler.h>
|
||||
#include <mission/devices/RadiationSensorHandler.h>
|
||||
#include <mission/devices/RwHandler.h>
|
||||
#include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
|
||||
#include <mission/devices/devicedefinitions/SyrlinksDefinitions.h>
|
||||
#include <mission/devices/devicedefinitions/PlocDefinitions.h>
|
||||
#include <mission/devices/devicedefinitions/RadSensorDefinitions.h>
|
||||
#include <mission/devices/devicedefinitions/Max31865Definitions.h>
|
||||
#include <mission/devices/devicedefinitions/RwDefinitions.h>
|
||||
#include <mission/utility/TmFunnel.h>
|
||||
#include <mission/obc/CCSDSIPCoreBridge.h>
|
||||
|
||||
@ -545,10 +548,13 @@ void ObjectFactory::produce(void* args){
|
||||
|
||||
UartCookie* plocUartCookie = new UartCookie(std::string("/dev/ttyUL3"), 115200,
|
||||
PLOC::MAX_REPLY_SIZE);
|
||||
PlocHandler* plocHandler = new PlocHandler(objects::PLOC_HANDLER, objects::UART_COM_IF,
|
||||
plocUartCookie);
|
||||
// plocHandler->setStartUpImmediately();
|
||||
(void) plocHandler;
|
||||
PlocHandler(objects::PLOC_HANDLER, objects::UART_COM_IF, plocUartCookie);
|
||||
|
||||
auto rw1SpiCookie = new SpiCookie(addresses::RW1, gpioIds::RW1, "/dev/spidev2.0",
|
||||
RwDefinitions::MAX_REPLY_SIZE, spi::RW_MODE, spi::RW_SPEED, &rwSpiCallback, nullptr);
|
||||
|
||||
auto rwHandler1 = new RwHandler(objects::RW1, objects::SPI_COM_IF, rw1SpiCookie);
|
||||
rwHandler1->setStartUpImmediately();
|
||||
|
||||
#endif /* TE0720 == 0 */
|
||||
|
||||
|
||||
3
bsp_q7s/spiCallbacks/CMakeLists.txt
Normal file
3
bsp_q7s/spiCallbacks/CMakeLists.txt
Normal file
@ -0,0 +1,3 @@
|
||||
target_sources(${TARGET_NAME} PRIVATE
|
||||
rwSpiCallback.cpp
|
||||
)
|
||||
220
bsp_q7s/spiCallbacks/rwSpiCallback.cpp
Normal file
220
bsp_q7s/spiCallbacks/rwSpiCallback.cpp
Normal file
@ -0,0 +1,220 @@
|
||||
#include <bsp_q7s/spiCallbacks/rwSpiCallback.h>
|
||||
#include <fsfw/serviceinterface/ServiceInterface.h>
|
||||
#include <mission/devices/RwHandler.h>
|
||||
#include <fsfw_hal/linux/spi/SpiCookie.h>
|
||||
#include <fsfw_hal/linux/UnixFileGuard.h>
|
||||
|
||||
ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData,
|
||||
size_t sendLen, void* args) {
|
||||
|
||||
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
|
||||
|
||||
RwHandler* handler = reinterpret_cast<RwHandler*>(args);
|
||||
if(handler == nullptr) {
|
||||
sif::error << "rwSpiCallback: Pointer to handler is invalid"
|
||||
<< std::endl;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
uint8_t writeBuffer[2];
|
||||
uint8_t writeSize = 0;
|
||||
|
||||
int fileDescriptor = 0;
|
||||
std::string device = cookie->getSpiDevice();
|
||||
UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "rwSpiCallback: ");
|
||||
if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "rwSpiCallback: Failed to open device file" << std::endl;
|
||||
return SpiComIF::OPENING_FILE_FAILED;
|
||||
}
|
||||
spi::SpiModes spiMode = spi::SpiModes::MODE_0;
|
||||
uint32_t spiSpeed = 0;
|
||||
cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
|
||||
comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
|
||||
|
||||
gpioId_t gpioId = cookie->getChipSelectPin();
|
||||
GpioIF* gpioIF = comIf->getGpioInterface();
|
||||
MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
|
||||
uint32_t timeoutMs = 0;
|
||||
MutexIF* mutex = comIf->getMutex(&timeoutType, &timeoutMs);
|
||||
if(mutex == nullptr or gpioIF == nullptr) {
|
||||
sif::debug << "rwSpiCallback: Mutex or GPIO interface invalid" << std::endl;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
if(gpioId != gpio::NO_GPIO) {
|
||||
result = mutex->lockMutex(timeoutType, timeoutMs);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::debug << "rwSpiCallback: Failed to lock mutex" << std::endl;
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
/** Sending frame start sign */
|
||||
writeBuffer[0] = 0x7E;
|
||||
writeSize = 1;
|
||||
|
||||
// Pull SPI CS low. For now, no support for active high given
|
||||
if(gpioId != gpio::NO_GPIO) {
|
||||
if(gpioIF->pullLow(gpioId) != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "rwSpiCallback: Failed to pull chip select low" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
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;;
|
||||
}
|
||||
return RwHandler::SPI_WRITE_FAILURE;
|
||||
}
|
||||
|
||||
/** Encoding and sending command */
|
||||
size_t idx = 0;
|
||||
while(idx < sendLen) {
|
||||
switch(*(sendData + idx)) {
|
||||
case 0x7E:
|
||||
writeBuffer[0] = 0x7D;
|
||||
writeBuffer[1] = 0x5E;
|
||||
writeSize = 2;
|
||||
break;
|
||||
case 0x7D:
|
||||
writeBuffer[0] = 0x7D;
|
||||
writeBuffer[1] = 0x5D;
|
||||
writeSize = 2;
|
||||
break;
|
||||
default:
|
||||
writeBuffer[0] = *(sendData + idx);
|
||||
writeSize = 1;
|
||||
}
|
||||
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";
|
||||
}
|
||||
return RwHandler::SPI_WRITE_FAILURE;
|
||||
}
|
||||
}
|
||||
|
||||
/** Sending frame start sign */
|
||||
writeBuffer[0] = 0x7E;
|
||||
writeSize = 1;
|
||||
|
||||
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";
|
||||
}
|
||||
return RwHandler::SPI_WRITE_FAILURE;
|
||||
}
|
||||
|
||||
uint8_t* rxBuf = nullptr;
|
||||
result = comIf->getReadBuffer(cookie->getSpiAddress(), &rxBuf);
|
||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
size_t replyBufferSize = cookie->getMaxBufferSize();
|
||||
|
||||
uint8_t byteRead = 0;
|
||||
/** Reading the reply frame */
|
||||
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
||||
sif::error << "rwSpiCallback: Failed to read first byte of reply frame" << std::endl;
|
||||
return RwHandler::SPI_READ_FAILURE;
|
||||
}
|
||||
|
||||
/** First byte must be the start sign 0x7E */
|
||||
if (byteRead != 0x7E) {
|
||||
sif::error << "rwSpiCallback: First byte of reply is not 0x7E" << std::endl;
|
||||
return RwHandler::MISSING_START_SIGN;
|
||||
}
|
||||
|
||||
size_t decodedFrameLen = 0;
|
||||
for (; decodedFrameLen < replyBufferSize; decodedFrameLen++) {
|
||||
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
||||
sif::error << "rwSpiCallback: Read failed" << std::endl;
|
||||
return RwHandler::SPI_READ_FAILURE;
|
||||
}
|
||||
|
||||
if (byteRead == 0x7E) {
|
||||
/** Reached end of frame */
|
||||
break;
|
||||
}
|
||||
else if (byteRead == 0xD) {
|
||||
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
||||
sif::error << "rwSpiCallback: Read failed" << std::endl;
|
||||
result = RwHandler::SPI_READ_FAILURE;
|
||||
break;
|
||||
}
|
||||
if (byteRead == 0x5E) {
|
||||
*(rxBuf + decodedFrameLen) = 0x7E;
|
||||
decodedFrameLen++;
|
||||
continue;
|
||||
}
|
||||
else if (byteRead == 0x5D) {
|
||||
*(rxBuf + decodedFrameLen) = 0x7D;
|
||||
decodedFrameLen++;
|
||||
continue;
|
||||
}
|
||||
else {
|
||||
sif::error << "rwSpiCallback: Invalid substitute" << std::endl;
|
||||
result = RwHandler::INVALID_SUBSTITUTE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
*(rxBuf + decodedFrameLen) = byteRead;
|
||||
decodedFrameLen++;
|
||||
continue;
|
||||
}
|
||||
|
||||
/**
|
||||
* There might be the unlikely case that each byte in a get-telemetry reply has been
|
||||
* replaced by its substitute. Than the next byte must correspond to the end sign 0x7E.
|
||||
* Otherwise there might be something wrong.
|
||||
*/
|
||||
if (decodedFrameLen == replyBufferSize) {
|
||||
if(read(fileDescriptor, &byteRead, 1) != 1) {
|
||||
sif::error << "rwSpiCallback: Failed to read last byte" << std::endl;
|
||||
result = RwHandler::SPI_READ_FAILURE;
|
||||
break;
|
||||
}
|
||||
if (byteRead != 0x7E) {
|
||||
sif::error << "rwSpiCallback: Missing end sign 0x7E" << std::endl;
|
||||
decodedFrameLen--;
|
||||
result = RwHandler::MISSING_END_SIGN;
|
||||
break;
|
||||
}
|
||||
}
|
||||
result = HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
if(gpioId != gpio::NO_GPIO) {
|
||||
if(mutex->unlockMutex() != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "rwSpiCallback: Failed to unlock mutex" << std::endl;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
21
bsp_q7s/spiCallbacks/rwSpiCallback.h
Normal file
21
bsp_q7s/spiCallbacks/rwSpiCallback.h
Normal file
@ -0,0 +1,21 @@
|
||||
#ifndef BSP_Q7S_RW_SPI_CALLBACK_H_
|
||||
#define BSP_Q7S_RW_SPI_CALLBACK_H_
|
||||
|
||||
#include <fsfw/returnvalues/HasReturnvaluesIF.h>
|
||||
#include <fsfw_hal/linux/spi/SpiComIF.h>
|
||||
|
||||
/**
|
||||
* @brief This is the callback function to send commands to the nano avionics reaction wheels and
|
||||
* receive the replies.
|
||||
*
|
||||
* @details The data to sent are additionally encoded according to the HDLC framing defined in the
|
||||
* datasheet of the reaction wheels:
|
||||
* https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
|
||||
* Arbeitsdaten/08_Used%20Components/Nanoavionics_Reactionwheels&fileid=181622
|
||||
* Each command entails exactly one reply which will also be read in and decoded by this
|
||||
* function.
|
||||
*/
|
||||
ReturnValue_t rwSpiCallback(SpiComIF* comIf, SpiCookie *cookie, const uint8_t *sendData,
|
||||
size_t sendLen, void* args);
|
||||
|
||||
#endif /* BSP_Q7S_RW_SPI_CALLBACK_H_ */
|
||||
@ -13,6 +13,7 @@ enum commonClassIds: uint8_t {
|
||||
HEATER_HANDLER, //HEATER
|
||||
SYRLINKS_HANDLER, //SYRLINKS
|
||||
IMTQ_HANDLER, //IMTQ
|
||||
RW_HANDLER, //Reaction Wheels
|
||||
PLOC_HANDLER, //PLOC
|
||||
SUS_HANDLER, //SUSS
|
||||
CCSDS_IP_CORE_BRIDGE, // IP Core interface
|
||||
|
||||
@ -13,6 +13,7 @@ enum: uint8_t {
|
||||
SA_DEPL_HANDLER = 110,
|
||||
PLOC_HANDLER = 111,
|
||||
IMTQ_HANDLER = 112,
|
||||
RW_HANDLER = 113,
|
||||
COMMON_SUBSYSTEM_ID_END
|
||||
};
|
||||
}
|
||||
|
||||
@ -26,6 +26,9 @@ static constexpr spi::SpiModes DEFAULT_MAX_1227_MODE = spi::SpiModes::MODE_3;
|
||||
static constexpr uint32_t DEFAULT_ADIS16507_SPEED = 976'000;
|
||||
static constexpr spi::SpiModes DEFAULT_ADIS16507_MODE = spi::SpiModes::MODE_3;
|
||||
|
||||
static constexpr uint32_t RW_SPEED = 300'000;
|
||||
static constexpr spi::SpiModes RW_MODE = spi::SpiModes::MODE_0;
|
||||
|
||||
}
|
||||
|
||||
#endif /* COMMON_CONFIG_SPICONF_H_ */
|
||||
|
||||
@ -63,7 +63,11 @@ namespace addresses {
|
||||
RTD_IC15,
|
||||
RTD_IC16,
|
||||
RTD_IC17,
|
||||
RTD_IC18
|
||||
RTD_IC18,
|
||||
RW1,
|
||||
RW2,
|
||||
RW3,
|
||||
RW4
|
||||
};
|
||||
|
||||
/* Addresses of devices supporting the CSP protocol */
|
||||
|
||||
@ -68,7 +68,12 @@ namespace gpioIds {
|
||||
CS_RAD_SENSOR,
|
||||
|
||||
PAPB_BUSY_N,
|
||||
PAPB_EMPTY
|
||||
PAPB_EMPTY,
|
||||
|
||||
RW1,
|
||||
RW2,
|
||||
RW3,
|
||||
RW4
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -58,6 +58,11 @@ namespace objects {
|
||||
|
||||
RAD_SENSOR = 0x54000050,
|
||||
|
||||
RW1 = 0x54000051,
|
||||
RW2 = 0x54000052,
|
||||
RW3 = 0x54000053,
|
||||
RW4 = 0x54000054,
|
||||
|
||||
/* 0x54 ('T') for test handlers */
|
||||
TEST_TASK = 0x54694269,
|
||||
LIBGPIOD_TEST = 0x54123456,
|
||||
|
||||
@ -16,6 +16,7 @@ target_sources(${TARGET_NAME} PUBLIC
|
||||
PlocHandler.cpp
|
||||
RadiationSensorHandler.cpp
|
||||
GyroADIS16507Handler.cpp
|
||||
RwHandler.cpp
|
||||
)
|
||||
|
||||
|
||||
|
||||
259
mission/devices/RwHandler.cpp
Normal file
259
mission/devices/RwHandler.cpp
Normal file
@ -0,0 +1,259 @@
|
||||
#include "RwHandler.h"
|
||||
#include "OBSWConfig.h"
|
||||
|
||||
#include <fsfw/globalfunctions/CRC.h>
|
||||
#include <fsfw/datapool/PoolReadGuard.h>
|
||||
#include <math.h>
|
||||
|
||||
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), statusSet(this) {
|
||||
if (comCookie == NULL) {
|
||||
sif::error << "RwHandler: Invalid com cookie" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
RwHandler::~RwHandler() {
|
||||
}
|
||||
|
||||
void RwHandler::doStartUp() {
|
||||
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
|
||||
setMode(MODE_NORMAL);
|
||||
#else
|
||||
setMode(_MODE_TO_ON);
|
||||
#endif
|
||||
}
|
||||
|
||||
void RwHandler::doShutDown() {
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
|
||||
switch (communicationStep) {
|
||||
case CommunicationStep::READ_TEMPERATURE:
|
||||
*id = RwDefinitions::GET_TEMPERATURE;
|
||||
break;
|
||||
case CommunicationStep::GET_RW_SATUS:
|
||||
*id = RwDefinitions::GET_RW_STATUS;
|
||||
break;
|
||||
default:
|
||||
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid communication step"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
return buildCommandFromCommand(*id, NULL, 0);
|
||||
}
|
||||
|
||||
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||
const uint8_t * commandData, size_t commandDataLen) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
|
||||
switch (deviceCommand) {
|
||||
case (RwDefinitions::GET_RW_STATUS): {
|
||||
prepareGetStatusCmd(commandData, commandDataLen);
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (RwDefinitions::SET_SPEED): {
|
||||
if (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);
|
||||
if (result != RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
prepareSetSpeedCmd(commandData, commandDataLen);
|
||||
return result;
|
||||
}
|
||||
case (RwDefinitions::GET_TEMPERATURE): {
|
||||
prepareGetTemperatureCmd();
|
||||
return RETURN_OK;
|
||||
}
|
||||
default:
|
||||
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
void RwHandler::fillCommandAndReplyMap() {
|
||||
this->insertInCommandAndReplyMap(RwDefinitions::GET_TEMPERATURE, 1, &temperatureSet,
|
||||
RwDefinitions::SIZE_GET_TEMPERATURE_REPLY);
|
||||
this->insertInCommandAndReplyMap(RwDefinitions::SET_SPEED, 1, nullptr,
|
||||
RwDefinitions::SIZE_SET_SPEED_REPLY);
|
||||
}
|
||||
|
||||
ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||
|
||||
switch (*(start + 1)) {
|
||||
case (static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS)): {
|
||||
*foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
|
||||
*foundId = RwDefinitions::GET_RW_STATUS;
|
||||
break;
|
||||
}
|
||||
case (static_cast<uint8_t>(RwDefinitions::SET_SPEED)): {
|
||||
*foundLen = RwDefinitions::SIZE_SET_SPEED_REPLY;
|
||||
*foundId = RwDefinitions::SET_SPEED;
|
||||
break;
|
||||
}
|
||||
case (static_cast<uint8_t>(RwDefinitions::GET_TEMPERATURE)): {
|
||||
*foundLen = RwDefinitions::SIZE_GET_TEMPERATURE_REPLY;
|
||||
*foundId = RwDefinitions::GET_TEMPERATURE;
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "RwHandler::scanForReply: Reply contains invalid command code" << std::endl;
|
||||
return RETURN_FAILED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
sizeOfReply = *foundLen;
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) {
|
||||
|
||||
/** Check result code */
|
||||
if (*(packet + 1) != 0) {
|
||||
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) {
|
||||
return CRC_ERROR;
|
||||
}
|
||||
|
||||
switch (id) {
|
||||
case (RwDefinitions::GET_RW_STATUS): {
|
||||
handleGetRwStatusReply(packet);
|
||||
break;
|
||||
}
|
||||
case (RwDefinitions::SET_SPEED):
|
||||
// no reply data expected
|
||||
break;
|
||||
case (RwDefinitions::GET_TEMPERATURE): {
|
||||
handleTemperatureReply(packet);
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::debug << "RwHandler::interpretDeviceReply: Unknown device reply id" << std::endl;
|
||||
return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
|
||||
}
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void RwHandler::setNormalDatapoolEntriesInvalid() {
|
||||
|
||||
}
|
||||
|
||||
uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
|
||||
return 5000;
|
||||
}
|
||||
|
||||
ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) {
|
||||
|
||||
localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>( { 0 }));
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void RwHandler::prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen) {
|
||||
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS);
|
||||
|
||||
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
|
||||
commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
|
||||
commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
|
||||
rawPacket = commandBuffer;
|
||||
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);
|
||||
|
||||
if (speed < -65000 || speed > 65000 || (speed > -1000 && speed < 1000)) {
|
||||
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid speed" << std::endl;
|
||||
return INVALID_SPEED;
|
||||
}
|
||||
|
||||
uint16_t rampTime = *commandData << 8 | *(commandData + 1);
|
||||
|
||||
if (rampTime < 10 || rampTime > 10000) {
|
||||
sif::error << "RwHandler::checkSpeedAndRampTime: Command has invalid ramp time"
|
||||
<< std::endl;
|
||||
return INVALID_RAMP_TIME;
|
||||
}
|
||||
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void RwHandler::prepareGetTemperatureCmd() {
|
||||
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_TEMPERATURE);
|
||||
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
|
||||
commandBuffer[1] = static_cast<uint8_t>(crc & 0xFF);
|
||||
commandBuffer[2] = static_cast<uint8_t>(crc >> 8 & 0xFF);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 3;
|
||||
}
|
||||
|
||||
|
||||
void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen) {
|
||||
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 + 1);
|
||||
commandBuffer[6] = *commandData;
|
||||
|
||||
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);
|
||||
rawPacket = commandBuffer;
|
||||
rawPacketLen = 9;
|
||||
}
|
||||
|
||||
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
|
||||
uint8_t offset = 0;
|
||||
statusSet.currSpeed = *(packet + 3) << 24 | *(packet + 2) << 16 | *(packet + 1) << 1 | *packet;
|
||||
offset += 4;
|
||||
statusSet.referenceSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16 | *(packet + offset + 1) << 1 | *(packet + offset);
|
||||
offset += 4;
|
||||
statusSet.state = *(packet + offset);
|
||||
offset += 1;
|
||||
statusSet.clcMode = *(packet + offset);
|
||||
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
||||
sif::info << "RwHandler::handleGetRwStatusReply: Current speed is: " << statusSet.currSpeed
|
||||
<< " * 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;
|
||||
#endif
|
||||
}
|
||||
|
||||
void RwHandler::handleTemperatureReply(const uint8_t* packet) {
|
||||
temperatureSet.temperatureCelcius = *(packet + 3) << 24 | *(packet + 2) << 16
|
||||
| *(packet + 1) << 1 | *packet;
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
||||
sif::info << "RwHandler::handleTemperatureReply: Temperature: "
|
||||
<< temperatureSet.temperatureCelcius << " °C" << std::endl;
|
||||
#endif
|
||||
}
|
||||
125
mission/devices/RwHandler.h
Normal file
125
mission/devices/RwHandler.h
Normal file
@ -0,0 +1,125 @@
|
||||
#ifndef MISSION_DEVICES_RWHANDLER_H_
|
||||
#define MISSION_DEVICES_RWHANDLER_H_
|
||||
|
||||
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
|
||||
#include <mission/devices/devicedefinitions/RwDefinitions.h>
|
||||
#include <string.h>
|
||||
|
||||
/**
|
||||
* @brief This is the device handler for the reaction wheel from nano avionics.
|
||||
*
|
||||
* @details Datasheet: https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
|
||||
* Arbeitsdaten/08_Used%20Components/Nanoavionics_Reactionwheels&fileid=181622
|
||||
*
|
||||
* @note Values are transferred in little endian format.
|
||||
*
|
||||
* @author J. Meier
|
||||
*/
|
||||
class RwHandler: public DeviceHandlerBase {
|
||||
public:
|
||||
|
||||
RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie);
|
||||
virtual ~RwHandler();
|
||||
|
||||
/**
|
||||
* @brief Sets mode to MODE_NORMAL. Can be used for debugging.
|
||||
*/
|
||||
void setModeNormal();
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
|
||||
|
||||
static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0);
|
||||
//! [EXPORT] : [COMMENT] Used by the spi send function to tell a failing read call
|
||||
static const ReturnValue_t SPI_READ_FAILURE = MAKE_RETURN_CODE(0xB1);
|
||||
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about a missing start sign 0x7E
|
||||
static const ReturnValue_t MISSING_START_SIGN = MAKE_RETURN_CODE(0xB2);
|
||||
//! [EXPORT] : [COMMENT] Can be used by the HDLC decoding mechanism to inform about an invalid substitution combination
|
||||
static const ReturnValue_t INVALID_SUBSTITUTE = MAKE_RETURN_CODE(0xB3);
|
||||
//! [EXPORT] : [COMMENT] HDLC decoding mechanism never receives the end sign 0x7E
|
||||
static const ReturnValue_t MISSING_END_SIGN = MAKE_RETURN_CODE(0xB4);
|
||||
|
||||
protected:
|
||||
void doStartUp() override;
|
||||
void doShutDown() override;
|
||||
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
|
||||
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override;
|
||||
void fillCommandAndReplyMap() override;
|
||||
ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand,
|
||||
const uint8_t * commandData,size_t commandDataLen) override;
|
||||
ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize,
|
||||
DeviceCommandId_t *foundId, size_t *foundLen) override;
|
||||
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
|
||||
const uint8_t *packet) override;
|
||||
void setNormalDatapoolEntriesInvalid() override;
|
||||
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
|
||||
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) override;
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::RW_HANDLER;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Action Message with invalid speed was received. Valid speeds must be in the range of [-65000; 1000] or [1000; 65000]
|
||||
static const ReturnValue_t INVALID_SPEED = MAKE_RETURN_CODE(0xA0);
|
||||
//! [EXPORT] : [COMMENT] Action Message with invalid ramp time was received.
|
||||
static const ReturnValue_t INVALID_RAMP_TIME = MAKE_RETURN_CODE(0xA1);
|
||||
//! [EXPORT] : [COMMENT] Received set speed command has invalid length. Should be 6.
|
||||
static const ReturnValue_t SET_SPEED_COMMAND_INVALID_LENGTH = MAKE_RETURN_CODE(0xA2);
|
||||
//! [EXPORT] : [COMMENT] Command execution failed
|
||||
static const ReturnValue_t EXECUTION_FAILED = MAKE_RETURN_CODE(0xA3);
|
||||
//! [EXPORT] : [COMMENT] Reaction wheel reply has invalid crc
|
||||
static const ReturnValue_t CRC_ERROR = MAKE_RETURN_CODE(0xA4);
|
||||
|
||||
RwDefinitions::TemperatureSet temperatureSet;
|
||||
RwDefinitions::StatusSet statusSet;
|
||||
|
||||
|
||||
uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
|
||||
|
||||
enum class CommunicationStep {
|
||||
READ_TEMPERATURE,
|
||||
GET_RW_SATUS
|
||||
};
|
||||
|
||||
CommunicationStep communicationStep = CommunicationStep::READ_TEMPERATURE;
|
||||
|
||||
size_t sizeOfReply = 0;
|
||||
|
||||
/**
|
||||
* @brief This function prepares the send buffer with the data to request the status of
|
||||
* the reaction wheel.
|
||||
*/
|
||||
void prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief This function checks if the receiced speed and ramp time to set are in a valid
|
||||
* range.
|
||||
* @return RETURN_OK if successful, otherwise error code.
|
||||
*/
|
||||
ReturnValue_t checkSpeedAndRampTime(const uint8_t * commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief This function prepares the set speed command from the commandData received with
|
||||
* an action message.
|
||||
*/
|
||||
void prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief This function fills the commandBuffer with the data to request the temperature.
|
||||
*/
|
||||
void prepareGetTemperatureCmd();
|
||||
|
||||
/**
|
||||
* @brief This function handles the reply of the get temperature command.
|
||||
*
|
||||
* @param packet Pointer to the reply data
|
||||
*/
|
||||
void handleTemperatureReply(const uint8_t* packet);
|
||||
|
||||
/**
|
||||
* @brief This function fills the status set with the data from the get-status-reply.
|
||||
*/
|
||||
void handleGetRwStatusReply(const uint8_t* packet);
|
||||
};
|
||||
|
||||
#endif /* MISSION_DEVICES_RWHANDLER_H_ */
|
||||
@ -1,5 +1,5 @@
|
||||
#ifndef MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_
|
||||
#define MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_
|
||||
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_
|
||||
#define MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_
|
||||
|
||||
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
|
||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||
@ -54,5 +54,5 @@ public:
|
||||
|
||||
}
|
||||
|
||||
#endif /* MISSION_DEVICES_DEVICEPACKETS_THERMALSENSORPACKET_H_ */
|
||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MAX13865DEFINITIONS_H_ */
|
||||
|
||||
|
||||
86
mission/devices/devicedefinitions/RwDefinitions.h
Normal file
86
mission/devices/devicedefinitions/RwDefinitions.h
Normal file
@ -0,0 +1,86 @@
|
||||
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_
|
||||
#define MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_
|
||||
|
||||
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
|
||||
#include <fsfw/datapoollocal/LocalPoolVariable.h>
|
||||
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
|
||||
#include "objects/systemObjectList.h"
|
||||
|
||||
namespace RwDefinitions {
|
||||
|
||||
enum PoolIds: lp_id_t {
|
||||
TEMPERATURE_C,
|
||||
CURR_SPEED,
|
||||
REFERENCE_SPEED,
|
||||
STATE,
|
||||
CLC_MODE
|
||||
};
|
||||
|
||||
static constexpr DeviceCommandId_t GET_RW_STATUS = 4;
|
||||
static constexpr DeviceCommandId_t SET_SPEED = 6;
|
||||
static constexpr DeviceCommandId_t GET_TEMPERATURE = 8;
|
||||
|
||||
static constexpr uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE;
|
||||
static constexpr uint32_t STATUS_SET_ID = GET_RW_STATUS;
|
||||
|
||||
static constexpr size_t SIZE_GET_RW_STATUS = 14;
|
||||
static constexpr size_t SIZE_SET_SPEED_REPLY = 4;
|
||||
static constexpr size_t SIZE_GET_TEMPERATURE_REPLY = 8;
|
||||
/** Max size when requesting telemetry */
|
||||
static constexpr size_t SIZE_GET_TELEMETRY_REPLY = 83;
|
||||
|
||||
/** Set speed command has maximum size */
|
||||
static constexpr size_t MAX_CMD_SIZE = 9;
|
||||
static constexpr size_t MAX_REPLY_SIZE = SIZE_GET_TELEMETRY_REPLY;
|
||||
|
||||
static constexpr uint8_t TEMPERATURE_SET_ENTRIES = 1;
|
||||
static constexpr uint8_t STATUS_SET_ENTRIES = 4;
|
||||
|
||||
/**
|
||||
* @brief This dataset can be used to store the temperature of a reaction wheel.
|
||||
*/
|
||||
class TemperatureSet:
|
||||
public StaticLocalDataSet<TEMPERATURE_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
TemperatureSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, TEMPERATURE_SET_ID) {
|
||||
}
|
||||
|
||||
TemperatureSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, TEMPERATURE_SET_ID)) {
|
||||
}
|
||||
|
||||
lp_var_t<int32_t> temperatureCelcius = lp_var_t<int32_t>(sid.objectId,
|
||||
PoolIds::TEMPERATURE_C, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This dataset can be used to store the reaction wheel status.
|
||||
*/
|
||||
class StatusSet:
|
||||
public StaticLocalDataSet<STATUS_SET_ENTRIES> {
|
||||
public:
|
||||
|
||||
StatusSet(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, STATUS_SET_ID) {
|
||||
}
|
||||
|
||||
StatusSet(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, STATUS_SET_ID)) {
|
||||
}
|
||||
|
||||
lp_var_t<int32_t> currSpeed = lp_var_t<int32_t>(sid.objectId,
|
||||
PoolIds::CURR_SPEED, this);
|
||||
lp_var_t<int32_t> referenceSpeed = lp_var_t<int32_t>(sid.objectId,
|
||||
PoolIds::REFERENCE_SPEED, this);
|
||||
lp_var_t<uint8_t> state = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::STATE, this);
|
||||
lp_var_t<uint8_t> clcMode = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::CLC_MODE, this);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user