added get reset and clear reset command to rw handler
This commit is contained in:
parent
3193d007c7
commit
ad43ebd587
@ -7,7 +7,7 @@
|
||||
RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie,
|
||||
LinuxLibgpioIF* gpioComIF, gpioId_t enableGpio) :
|
||||
DeviceHandlerBase(objectId, comIF, comCookie), gpioComIF(gpioComIF), enableGpio(enableGpio),
|
||||
temperatureSet(this), statusSet(this) {
|
||||
temperatureSet(this), statusSet(this), lastResetStatusSet(this) {
|
||||
if (comCookie == NULL) {
|
||||
sif::error << "RwHandler: Invalid com cookie" << std::endl;
|
||||
}
|
||||
@ -20,6 +20,9 @@ RwHandler::~RwHandler() {
|
||||
}
|
||||
|
||||
void RwHandler::doStartUp() {
|
||||
if (!startupPerformed) {
|
||||
return;
|
||||
}
|
||||
if(gpioComIF->pullHigh(enableGpio) != RETURN_OK) {
|
||||
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
|
||||
}
|
||||
@ -34,6 +37,7 @@ void RwHandler::doShutDown() {
|
||||
if(gpioComIF->pullLow(enableGpio) != RETURN_OK) {
|
||||
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
|
||||
}
|
||||
startupPerformed = false;
|
||||
}
|
||||
|
||||
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
|
||||
@ -55,7 +59,25 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
|
||||
}
|
||||
|
||||
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
|
||||
*id = RwDefinitions::INIT_RW_CONTROLLER;
|
||||
switch (startupStep) {
|
||||
case StartupStep::GET_RESET_STATUS:
|
||||
*id = RwDefinitions::GET_LAST_RESET_STATUS;
|
||||
startupStep = StartupStep::CLEAR_RESET_STATUS;
|
||||
break;
|
||||
case StartupStep::CLEAR_RESET_STATUS:
|
||||
*id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
|
||||
startupStep = StartupStep::INIT_RW;
|
||||
break;
|
||||
case StartupStep::INIT_RW:
|
||||
*id = RwDefinitions::INIT_RW_CONTROLLER;
|
||||
startupStep = StartupStep::GET_RESET_STATUS;
|
||||
startupPerformed = true;
|
||||
break;
|
||||
default:
|
||||
sif::debug << "RwHandler::buildTransitionDeviceCommand: Invalid startup step"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
return buildCommandFromCommand(*id, nullptr, 0);;
|
||||
}
|
||||
|
||||
@ -70,6 +92,14 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
|
||||
rawPacketLen = 1;
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (RwDefinitions::GET_LAST_RESET_STATUS): {
|
||||
prepareGetLastResetStatusCommand();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (RwDefinitions::CLEAR_LAST_RESET_STATUS): {
|
||||
prepareClearResetStatusCommand();
|
||||
return RETURN_OK;
|
||||
}
|
||||
case (RwDefinitions::GET_RW_STATUS): {
|
||||
prepareGetStatusCmd(commandData, commandDataLen);
|
||||
return RETURN_OK;
|
||||
@ -103,6 +133,9 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
|
||||
|
||||
void RwHandler::fillCommandAndReplyMap() {
|
||||
this->insertInCommandMap(RwDefinitions::RESET_MCU);
|
||||
this->insertInCommandAndReplyMap(RwDefinitions::GET_LAST_RESET_STATUS, 1, &lastResetStatusSet,
|
||||
RwDefinitions::SIZE_GET_RESET_STATUS);
|
||||
this->insertInCommandMap(RwDefinitions::CLEAR_LAST_RESET_STATUS);
|
||||
this->insertInCommandAndReplyMap(RwDefinitions::GET_RW_STATUS, 1, &statusSet,
|
||||
RwDefinitions::SIZE_GET_RW_STATUS);
|
||||
this->insertInCommandMap(RwDefinitions::INIT_RW_CONTROLLER);
|
||||
@ -116,6 +149,11 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t *start, size_t remainingSize
|
||||
DeviceCommandId_t *foundId, size_t *foundLen) {
|
||||
|
||||
switch (*(start)) {
|
||||
case (static_cast<uint8_t>(RwDefinitions::GET_LAST_RESET_STATUS)): {
|
||||
*foundLen = RwDefinitions::SIZE_GET_RESET_STATUS;
|
||||
*foundId = RwDefinitions::GET_LAST_RESET_STATUS;
|
||||
break;
|
||||
}
|
||||
case (static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS)): {
|
||||
*foundLen = RwDefinitions::SIZE_GET_RW_STATUS;
|
||||
*foundId = RwDefinitions::GET_RW_STATUS;
|
||||
@ -159,6 +197,10 @@ ReturnValue_t RwHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_
|
||||
}
|
||||
|
||||
switch (id) {
|
||||
case (RwDefinitions::GET_LAST_RESET_STATUS): {
|
||||
handleResetStatusReply(packet);
|
||||
break;
|
||||
}
|
||||
case (RwDefinitions::GET_RW_STATUS): {
|
||||
handleGetRwStatusReply(packet);
|
||||
break;
|
||||
@ -195,6 +237,24 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
void RwHandler::prepareGetLastResetStatusCommand() {
|
||||
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_LAST_RESET_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;
|
||||
}
|
||||
|
||||
void RwHandler::prepareClearResetStatusCommand() {
|
||||
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::CLEAR_LAST_RESET_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;
|
||||
}
|
||||
|
||||
void RwHandler::prepareGetStatusCmd(const uint8_t * commandData, size_t commandDataLen) {
|
||||
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_RW_STATUS);
|
||||
|
||||
@ -225,15 +285,6 @@ ReturnValue_t RwHandler::checkSpeedAndRampTime(const uint8_t* commandData, size_
|
||||
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::prepareInitRwCommand() {
|
||||
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::INIT_RW_CONTROLLER);
|
||||
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
|
||||
@ -243,6 +294,14 @@ void RwHandler::prepareInitRwCommand() {
|
||||
rawPacketLen = 3;
|
||||
}
|
||||
|
||||
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);
|
||||
@ -263,6 +322,15 @@ void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDa
|
||||
rawPacketLen = 9;
|
||||
}
|
||||
|
||||
void RwHandler::handleResetStatusReply(const uint8_t* packet) {
|
||||
uint8_t offset = 2;
|
||||
lastResetStatusSet.lastResetStatus = *(packet + offset);
|
||||
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
|
||||
sif::info << "RwHandler::handleResetStatusReply: Last reset status: "
|
||||
<< static_cast<unsigned int>(lastResetStatusSet.lastResetStatus.value) << std::endl;
|
||||
#endif
|
||||
}
|
||||
|
||||
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
|
||||
uint8_t offset = 2;
|
||||
statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
|
||||
|
@ -84,6 +84,7 @@ private:
|
||||
|
||||
RwDefinitions::TemperatureSet temperatureSet;
|
||||
RwDefinitions::StatusSet statusSet;
|
||||
RwDefinitions::LastResetSatus lastResetStatusSet;
|
||||
|
||||
|
||||
uint8_t commandBuffer[RwDefinitions::MAX_CMD_SIZE];
|
||||
@ -95,8 +96,28 @@ private:
|
||||
|
||||
CommunicationStep communicationStep = CommunicationStep::READ_TEMPERATURE;
|
||||
|
||||
enum class StartupStep {
|
||||
GET_RESET_STATUS,
|
||||
CLEAR_RESET_STATUS,
|
||||
INIT_RW
|
||||
};
|
||||
|
||||
StartupStep startupStep = StartupStep::GET_RESET_STATUS;
|
||||
|
||||
size_t sizeOfReply = 0;
|
||||
|
||||
bool startupPerformed = false;
|
||||
|
||||
/**
|
||||
* @brief This function prepares the command to request the last reset status
|
||||
*/
|
||||
void prepareGetLastResetStatusCommand();
|
||||
|
||||
/**
|
||||
* @brief Fills the command buffer with the command to clear the reset status.
|
||||
*/
|
||||
void prepareClearResetStatusCommand();
|
||||
|
||||
/**
|
||||
* @brief This function prepares the send buffer with the data to request the status of
|
||||
* the reaction wheel.
|
||||
@ -110,6 +131,13 @@ private:
|
||||
*/
|
||||
ReturnValue_t checkSpeedAndRampTime(const uint8_t * commandData, size_t commandDataLen);
|
||||
|
||||
/**
|
||||
* @brief This function fills the commandBuffer with the data to request initialize the
|
||||
* reaction wheel controller. This command must be sent as soon as the state of a
|
||||
* reaction wheel is equal to 1 which indicates an error.
|
||||
*/
|
||||
void prepareInitRwCommand();
|
||||
|
||||
/**
|
||||
* @brief This function prepares the set speed command from the commandData received with
|
||||
* an action message.
|
||||
@ -122,11 +150,12 @@ private:
|
||||
void prepareGetTemperatureCmd();
|
||||
|
||||
/**
|
||||
* @brief This function fills the commandBuffer with the data to request initialize the
|
||||
* reaction wheel controller. This command must be sent as soon as the state of a
|
||||
* reaction wheel is equal to 1 which indicates an error.
|
||||
* @brief This function writes the last reset status retrieved with the get last reset status
|
||||
* command into the reset status dataset.
|
||||
*
|
||||
* @param packet Pointer to the buffer holding the reply data.
|
||||
*/
|
||||
void prepareInitRwCommand();
|
||||
void handleResetStatusReply(const uint8_t* packet);
|
||||
|
||||
/**
|
||||
* @brief This function handles the reply of the get temperature command.
|
||||
|
@ -15,7 +15,8 @@ enum PoolIds: lp_id_t {
|
||||
CURR_SPEED,
|
||||
REFERENCE_SPEED,
|
||||
STATE,
|
||||
CLC_MODE
|
||||
CLC_MODE,
|
||||
LAST_RESET_STATUS
|
||||
};
|
||||
|
||||
enum States: uint8_t {
|
||||
@ -30,13 +31,16 @@ static const DeviceCommandId_t RESET_MCU = 1;
|
||||
static const DeviceCommandId_t GET_LAST_RESET_STATUS = 2;
|
||||
static const DeviceCommandId_t CLEAR_LAST_RESET_STATUS = 3;
|
||||
static const DeviceCommandId_t GET_RW_STATUS = 4;
|
||||
/** This command is needed to recover from error state */
|
||||
static const DeviceCommandId_t INIT_RW_CONTROLLER = 5;
|
||||
static const DeviceCommandId_t SET_SPEED = 6;
|
||||
static const DeviceCommandId_t GET_TEMPERATURE = 8;
|
||||
|
||||
static const uint32_t TEMPERATURE_SET_ID = GET_TEMPERATURE;
|
||||
static const uint32_t STATUS_SET_ID = GET_RW_STATUS;
|
||||
static const uint32_t LAST_RESET_ID = GET_LAST_RESET_STATUS;
|
||||
|
||||
static const size_t SIZE_GET_RESET_STATUS = 5;
|
||||
static const size_t SIZE_GET_RW_STATUS = 14;
|
||||
static const size_t SIZE_SET_SPEED_REPLY = 4;
|
||||
static const size_t SIZE_GET_TEMPERATURE_REPLY = 8;
|
||||
@ -47,6 +51,7 @@ static const size_t SIZE_GET_TELEMETRY_REPLY = 83;
|
||||
static const size_t MAX_CMD_SIZE = 9;
|
||||
static const size_t MAX_REPLY_SIZE = SIZE_GET_TELEMETRY_REPLY;
|
||||
|
||||
static const uint8_t LAST_RESET_ENTRIES = 1;
|
||||
static const uint8_t TEMPERATURE_SET_ENTRIES = 1;
|
||||
static const uint8_t STATUS_SET_ENTRIES = 4;
|
||||
|
||||
@ -94,6 +99,25 @@ public:
|
||||
PoolIds::CLC_MODE, this);
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief This dataset stores the last reset status.
|
||||
*/
|
||||
class LastResetSatus:
|
||||
public StaticLocalDataSet<LAST_RESET_ENTRIES> {
|
||||
public:
|
||||
|
||||
LastResetSatus(HasLocalDataPoolIF* owner):
|
||||
StaticLocalDataSet(owner, LAST_RESET_ID) {
|
||||
}
|
||||
|
||||
LastResetSatus(object_id_t objectId):
|
||||
StaticLocalDataSet(sid_t(objectId, LAST_RESET_ID)) {
|
||||
}
|
||||
|
||||
lp_var_t<uint8_t> lastResetStatus = lp_var_t<uint8_t>(sid.objectId,
|
||||
PoolIds::LAST_RESET_STATUS, this);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWDEFINITIONS_H_ */
|
||||
|
Loading…
Reference in New Issue
Block a user