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

all reaction wheels working

Browse Source
This commit is contained in:
Jakob Meier
2021-06-29 16:08:20 +02:00
parent 703f29af26
commit 6e70f2254d
6 changed files with 86 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
mission/devices/RwHandler.cpp
View File

@ -20,18 +20,12 @@ RwHandler::~RwHandler() {
}
void RwHandler::doStartUp() {
switch (startupStep) {
case StartupStep::ENABLE_RW: {
if(gpioComIF->pullHigh(enableGpio) != RETURN_OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
}
return;
}
case StartupStep::STARTUP_COMPLETE:
break;
default:
return;
}
internalState = InternalState::GET_RESET_STATUS;
if(gpioComIF->pullHigh(enableGpio) != RETURN_OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to high";
}
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
setMode(MODE_NORMAL);
@ -44,23 +38,26 @@ void RwHandler::doShutDown() {
if(gpioComIF->pullLow(enableGpio) != RETURN_OK) {
sif::debug << "RwHandler::doStartUp: Failed to pull enable gpio to low";
}
/** Reset startup step for next doStartUp routine */
startupStep = StartupStep::ENABLE_RW;
}
ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
switch (communicationStep) {
case CommunicationStep::GET_RESET_STATUS:
switch (internalState) {
case InternalState::GET_RESET_STATUS:
*id = RwDefinitions::GET_LAST_RESET_STATUS;
communicationStep = CommunicationStep::READ_TEMPERATURE;
internalState = InternalState::READ_TEMPERATURE;
break;
case CommunicationStep::READ_TEMPERATURE:
case InternalState::CLEAR_RESET_STATUS:
*id = RwDefinitions::CLEAR_LAST_RESET_STATUS;
/** After reset status is cleared, reset status will be polled again for verification */
internalState = InternalState::GET_RESET_STATUS;
break;
case InternalState::READ_TEMPERATURE:
*id = RwDefinitions::GET_TEMPERATURE;
communicationStep = CommunicationStep::GET_RW_SATUS;
internalState = InternalState::GET_RW_SATUS;
break;
case CommunicationStep::GET_RW_SATUS:
case InternalState::GET_RW_SATUS:
*id = RwDefinitions::GET_RW_STATUS;
communicationStep = CommunicationStep::GET_RESET_STATUS;
internalState = InternalState::GET_RESET_STATUS;
break;
default:
sif::debug << "RwHandler::buildNormalDeviceCommand: Invalid communication step"
@ -71,27 +68,7 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
}
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
switch (startupStep) {
case StartupStep::ENABLE_RW:
*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::STARTUP_COMPLETE;
break;
case StartupStep::STARTUP_COMPLETE:
break;
default:
sif::debug << "RwHandler::buildTransitionDeviceCommand: Invalid startup step"
<< std::endl;
break;
}
return buildCommandFromCommand(*id, nullptr, 0);;
return RETURN_OK;
}
ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
@ -100,9 +77,7 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
switch (deviceCommand) {
case (RwDefinitions::RESET_MCU): {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::RESET_MCU);
rawPacket = commandBuffer;
rawPacketLen = 1;
prepareResetMcuCommand();
return RETURN_OK;
}
case (RwDefinitions::GET_LAST_RESET_STATUS): {
@ -255,7 +230,7 @@ void RwHandler::setNormalDatapoolEntriesInvalid() {
}
uint32_t RwHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) {
return 15000;
return 500;
}
ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@ -263,9 +238,23 @@ ReturnValue_t RwHandler::initializeLocalDataPool(localpool::DataPool& localDataP
localDataPoolMap.emplace(RwDefinitions::TEMPERATURE_C, new PoolEntry<int32_t>( { 0 }));
localDataPoolMap.emplace(RwDefinitions::CURR_SPEED, new PoolEntry<int32_t>( { 0 }));
localDataPoolMap.emplace(RwDefinitions::REFERENCE_SPEED, new PoolEntry<int32_t>( { 0 }));
localDataPoolMap.emplace(RwDefinitions::STATE, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(RwDefinitions::CLC_MODE, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(RwDefinitions::LAST_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
localDataPoolMap.emplace(RwDefinitions::CURRRENT_RESET_STATUS, new PoolEntry<uint8_t>( { 0 }));
return RETURN_OK;
}
void RwHandler::prepareResetMcuCommand() {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::RESET_MCU);
rawPacket = commandBuffer;
rawPacketLen = 1;
}
void RwHandler::prepareGetLastResetStatusCommand() {
commandBuffer[0] = static_cast<uint8_t>(RwDefinitions::GET_LAST_RESET_STATUS);
uint16_t crc = CRC::crc16ccitt(commandBuffer, 1, 0xFFFF);
@ -352,15 +341,24 @@ void RwHandler::prepareSetSpeedCmd(const uint8_t * commandData, size_t commandDa
}
void RwHandler::handleResetStatusReply(const uint8_t* packet) {
PoolReadGuard rg(&lastResetStatusSet);
uint8_t offset = 2;
lastResetStatusSet.lastResetStatus = *(packet + offset);
uint8_t resetStatus = *(packet + offset);
if (resetStatus != RwDefinitions::CLEARED) {
internalState = InternalState::CLEAR_RESET_STATUS;
lastResetStatusSet.lastResetStatus = resetStatus;
}
lastResetStatusSet.currentResetStatus = resetStatus;
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
sif::info << "RwHandler::handleResetStatusReply: Last reset status: "
<< static_cast<unsigned int>(lastResetStatusSet.lastResetStatus.value) << std::endl;
sif::info << "RwHandler::handleResetStatusReply: Current reset status: "
<< static_cast<unsigned int>(lastResetStatusSet.currentResetStatus.value) << std::endl;
#endif
}
void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
PoolReadGuard rg(&statusSet);
uint8_t offset = 2;
statusSet.currSpeed = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);
@ -372,6 +370,16 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
offset += 1;
statusSet.clcMode = *(packet + offset);
if (statusSet.state == RwDefinitions::ERROR) {
/**
* This requires the commanding of the init reaction wheel controller command to recover
* form error state which must be handled by the FDIR instance.
*/
triggerEvent(ERROR_STATE);
sif::error << "RwHandler::handleGetRwStatusReply: Reaction wheel in error state"
<< std::endl;
}
#if OBSW_VERBOSE_LEVEL >= 1 && RW_DEBUG == 1
sif::info << "RwHandler::handleGetRwStatusReply: Current speed is: " << statusSet.currSpeed
<< " * 0.1 RPM" << std::endl;
@ -385,6 +393,7 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
}
void RwHandler::handleTemperatureReply(const uint8_t* packet) {
PoolReadGuard rg(&temperatureSet);
uint8_t offset = 2;
temperatureSet.temperatureCelcius = *(packet + offset + 3) << 24 | *(packet + offset + 2) << 16
| *(packet + offset + 1) << 8 | *(packet + offset);