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

RW shutdown #580

Merged
muellerr merged 6 commits from feature_proper_rw_shutdown into develop 2023-04-07 17:20:31 +02:00
Conversation 3 Commits 6 Files Changed 8 +84 -69
7 changed files with 84 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit cd8bbaf1f9 - Show all commits

2
bsp_q7s/core/scheduling.cpp
View File

@ -237,7 +237,7 @@ void scheduling::initTasks() {
#if OBSW_ADD_RW == 1
PeriodicTaskIF* rwPolling =
factory->createPeriodicTask("RW_POLLING_TASK", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2,
factory->createPeriodicTask("RW_POLLING_TASK", 75, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2,
0.4, missedDeadlineFunc, &RR_SCHEDULING);
result = rwPolling->addComponent(objects::RW_POLLING_TASK);
if (result != returnvalue::OK) {

59
linux/acs/RwPollingTask.cpp
View File

@ -11,6 +11,7 @@
#include <unistd.h>
#include "devConf.h"
#include "mission/acs/defs.h"
#include "mission/acs/rwHelpers.h"
RwPollingTask::RwPollingTask(object_id_t objectId, const char* spiDev, GpioIF& gpioIF)
@ -35,6 +36,7 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
semaphore->acquire();
// This loop takes 50 ms on a debug build.
// Stopwatch watch;
// Give all device handlers some time to submit requests.
TaskFactory::delayTask(5);
int fd = 0;
for (auto& skip : skipCommandingForRw) {
@ -46,12 +48,14 @@ ReturnValue_t RwPollingTask::performOperation(uint8_t operationCode) {
continue;
}
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
if (rwCookies[idx]->specialRequest == rws::SpecialRwRequest::RESET_MCU) {
if (rwRequests[idx].mode == acs::SimpleSensorMode::OFF) {
skipCommandingForRw[idx] = true;
} else if (rwRequests[idx].specialRequest == rws::SpecialRwRequest::RESET_MCU) {
prepareSimpleCommand(rws::RESET_MCU);
// No point in commanding that specific RW for the cycle.
skipCommandingForRw[idx] = true;
writeOneRwCmd(idx, fd);
} else if (rwCookies[idx]->setSpeed) {
} else if (rwRequests[idx].setSpeed) {
prepareSetSpeedCmd(idx);
if (writeOneRwCmd(idx, fd) != returnvalue::OK) {
continue;
@ -121,31 +125,14 @@ ReturnValue_t RwPollingTask::initializeInterface(CookieIF* cookie) {
ReturnValue_t RwPollingTask::sendMessage(CookieIF* cookie, const uint8_t* sendData,
size_t sendLen) {
if (sendData == nullptr or sendLen < 8) {
if (sendData == nullptr or sendLen != sizeof(rws::RwRequest)) {
return DeviceHandlerIF::INVALID_DATA;
}
int32_t speed = 0;
uint16_t rampTime = 0;
const uint8_t* currentBuf = sendData;
bool setSpeed = currentBuf[0];
currentBuf += 1;
sendLen -= 1;
SerializeAdapter::deSerialize(&speed, &currentBuf, &sendLen, SerializeIF::Endianness::MACHINE);
SerializeAdapter::deSerialize(&rampTime, &currentBuf, &sendLen, SerializeIF::Endianness::MACHINE);
rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
if (sendLen == 8 and sendData[7] < static_cast<uint8_t>(rws::SpecialRwRequest::NUM_REQUESTS)) {
specialRequest = static_cast<rws::SpecialRwRequest>(sendData[7]);
}
RwCookie* rwCookie = dynamic_cast<RwCookie*>(cookie);
if (rwCookie == nullptr) {
return returnvalue::FAILED;
}
const rws::RwRequest* rwRequest = reinterpret_cast<const rws::RwRequest*>(sendData);
uint8_t rwIdx = rwRequest->rwIdx;
{
MutexGuard mg(ipcLock);
rwCookie->setSpeed = setSpeed;
rwCookie->currentRwSpeed = speed;
rwCookie->currentRampTime = rampTime;
rwCookie->specialRequest = specialRequest;
std::memcpy(&rwRequests[rwIdx], rwRequest, sizeof(rws::RwRequest));
if (state == InternalState::IDLE) {
state = InternalState::IS_BUSY;
semaphore->release();
@ -332,7 +319,7 @@ ReturnValue_t RwPollingTask::writeOneRwCmd(uint8_t rwIdx, int fd) {
ReturnValue_t RwPollingTask::readAllRws(DeviceCommandId_t id) {
// SPI dev will be opened in readNextReply on demand.
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
if (((id == rws::SET_SPEED) and !rwCookies[idx]->setSpeed) or skipCommandingForRw[idx]) {
if (((id == rws::SET_SPEED) and !rwRequests[idx].setSpeed) or skipCommandingForRw[idx]) {
continue;
}
uint8_t* replyBuf;
@ -395,7 +382,7 @@ void RwPollingTask::fillSpecialRequestArray() {
specialRequestIds[idx] = DeviceHandlerIF::NO_COMMAND_ID;
continue;
}
switch (rwCookies[idx]->specialRequest) {
switch (rwRequests[idx].specialRequest) {
case (rws::SpecialRwRequest::GET_TM): {
specialRequestIds[idx] = rws::GET_TM;
break;
@ -426,14 +413,15 @@ void RwPollingTask::handleSpecialRequests() {
writeOneRwCmd(idx, fd);
}
closeSpi(fd);
usleep(rws::SPI_REPLY_DELAY);
// usleep(rws::SPI_REPLY_DELAY);
for (unsigned idx = 0; idx < rwCookies.size(); idx++) {
if (specialRequestIds[idx] == DeviceHandlerIF::NO_COMMAND_ID) {
continue;
}
uint8_t* replyBuf;
size_t maxReadLen = idAndIdxToReadBuffer(specialRequestIds[idx], idx, &replyBuf);
result = readNextReply(*rwCookies[idx], replyBuf, maxReadLen);
// Skip first byte for actual read buffer: Valid byte
result = readNextReply(*rwCookies[idx], replyBuf + 1, maxReadLen);
if (result == returnvalue::OK) {
// The first byte is always a flag which shows whether the value was read
// properly at least once.
@ -455,17 +443,12 @@ void RwPollingTask::setAllReadFlagsFalse() {
}
}
// This closes the SPI
void RwPollingTask::closeSpi(int fd) {
// This will perform the function to close the SPI
close(fd);
// The SPI is now closed.
}
void RwPollingTask::closeSpi(int fd) { close(fd); }
ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) {
gpioId_t gpioId = rwCookie.getChipSelectPin();
if (spiLock == nullptr) {
sif::debug << "rwSpiCallback::spiCallback: Mutex or GPIO interface invalid" << std::endl;
sif::warning << "RwPollingTask: Mutex or GPIO interface invalid" << std::endl;
return returnvalue::FAILED;
}
// Add datalinklayer like specified in the datasheet.
@ -473,7 +456,7 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) {
rws::encodeHdlc(writeBuffer.data(), writeLen, encodedBuffer.data(), lenToSend);
pullCsLow(gpioId, gpioIF);
if (write(fd, encodedBuffer.data(), lenToSend) != static_cast<ssize_t>(lenToSend)) {
sif::error << "rwSpiCallback::spiCallback: Write failed!" << std::endl;
sif::error << "RwPollingTask: Write failed!" << std::endl;
pullCsHigh(gpioId, gpioIF);
return rws::SPI_WRITE_FAILURE;
}
@ -484,7 +467,7 @@ ReturnValue_t RwPollingTask::sendOneMessage(int fd, RwCookie& rwCookie) {
ReturnValue_t RwPollingTask::pullCsLow(gpioId_t gpioId, GpioIF& gpioIF) {
ReturnValue_t result = spiLock->lockMutex(TIMEOUT_TYPE, TIMEOUT_MS);
if (result != returnvalue::OK) {
sif::debug << "RwPollingTask::pullCsLow: Failed to lock mutex" << std::endl;
sif::warning << "RwPollingTask::pullCsLow: Failed to lock mutex" << std::endl;
return result;
}
// Pull SPI CS low. For now, no support for active high given
@ -525,8 +508,8 @@ ReturnValue_t RwPollingTask::prepareSetSpeedCmd(uint8_t rwIdx) {
uint16_t rampTimeToSet = 10;
{
MutexGuard mg(ipcLock);
speedToSet = rwCookies[rwIdx]->currentRwSpeed;
rampTimeToSet = rwCookies[rwIdx]->currentRampTime;
speedToSet = rwRequests[rwIdx].currentRwSpeed;
rampTimeToSet = rwRequests[rwIdx].currentRampTime;
}
size_t serLen = 1;
SerializeAdapter::serialize(&speedToSet, &serPtr, &serLen, writeBuffer.size(),

6
linux/acs/RwPollingTask.h
View File

@ -8,6 +8,7 @@
#include <fsfw_hal/common/gpio/GpioIF.h>
#include <fsfw_hal/linux/spi/SpiComIF.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>
#include <mission/acs/defs.h>
#include "mission/acs/rwHelpers.h"
@ -26,10 +27,6 @@ class RwCookie : public SpiCookie {
std::array<uint8_t, REPLY_BUF_LEN> replyBuf{};
std::array<uint8_t, REPLY_BUF_LEN> exchangeBuf{};
MutexIF* bufLock;
bool setSpeed = true;
int32_t currentRwSpeed = 0;
uint16_t currentRampTime = 0;
rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
uint8_t rwIdx;
};
@ -52,6 +49,7 @@ class RwPollingTask : public SystemObject, public ExecutableObjectIF, public Dev
std::array<bool, 4> skipCommandingForRw;
std::array<DeviceCommandId_t, 4> specialRequestIds;
std::array<RwCookie*, 4> rwCookies;
std::array<rws::RwRequest, 4> rwRequests{};
std::array<uint8_t, rws::MAX_CMD_SIZE> writeBuffer;
std::array<uint8_t, rws::MAX_CMD_SIZE * 2> encodedBuffer;

66
mission/acs/RwHandler.cpp
View File

@ -4,6 +4,7 @@
#include <fsfw/globalfunctions/CRC.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw_hal/common/gpio/GpioIF.h>
#include <mission/acs/defs.h>
#include "OBSWConfig.h"
@ -23,7 +24,9 @@ RwHandler::RwHandler(object_id_t objectId, object_id_t comIF, CookieIF* comCooki
if (gpioComIF == nullptr) {
sif::error << "RwHandler: Invalid gpio communication interface" << std::endl;
}
currentRequest.rwIdx = rwIdx;
}
RwHandler::~RwHandler() {}
void RwHandler::doStartUp() {
@ -32,6 +35,7 @@ void RwHandler::doStartUp() {
if (gpioComIF->pullHigh(enableGpio) != returnvalue::OK) {
sif::error << "RW Handler " << rwIdx << ": Failed to pull ENABLE pin high for startup";
}
currentRequest.mode = acs::SimpleSensorMode::NORMAL;
updatePeriodicReply(true, rws::REPLY_ID);
statusSet.setReportingEnabled(true);
setMode(_MODE_TO_ON);
@ -40,11 +44,11 @@ void RwHandler::doStartUp() {
void RwHandler::doShutDown() {
if (internalState != InternalState::SHUTDOWN) {
internalState = InternalState::SHUTDOWN;
commandExecuted = false;
shutdownState = ShutdownState::SET_SPEED_ZERO;
offTransitionCountdown.resetTimer();
}
if (((internalState == InternalState::SHUTDOWN) and commandExecuted) or
offTransitionCountdown.hasTimedOut()) {
if ((internalState == InternalState::SHUTDOWN) and
(shutdownState == ShutdownState::DONE or offTransitionCountdown.hasTimedOut())) {
if (gpioComIF->pullLow(enableGpio) != returnvalue::OK) {
sif::error << "RW Handler " << rwIdx << ": Failed to pull ENABLE pin low for shutdown";
}
@ -61,8 +65,8 @@ void RwHandler::doShutDown() {
PoolReadGuard pg(&tmDataset);
tmDataset.setValidity(false, true);
}
commandExecuted = false;
internalState = InternalState::DEFAULT;
shutdownState = ShutdownState::NONE;
// The power switch is handled by the assembly, so we can go off here directly.
setMode(MODE_OFF);
}
@ -83,12 +87,18 @@ ReturnValue_t RwHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) {
ReturnValue_t RwHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) {
if (internalState == InternalState::SHUTDOWN) {
{
PoolReadGuard pg(&rwSpeedActuationSet);
rwSpeedActuationSet.setRwSpeed(0, 10);
if (shutdownState == ShutdownState::SET_SPEED_ZERO) {
{
PoolReadGuard pg(&rwSpeedActuationSet);
rwSpeedActuationSet.setRwSpeed(0, 10);
}
*id = rws::REQUEST_ID;
return buildCommandFromCommand(*id, nullptr, 0);
} else if (shutdownState == ShutdownState::STOP_POLLING) {
currentRequest.mode = acs::SimpleSensorMode::OFF;
*id = rws::REQUEST_ID;
return buildCommandFromCommand(*id, nullptr, 0);
}
*id = rws::REQUEST_ID;
return buildCommandFromCommand(*id, nullptr, 0);
}
return NOTHING_TO_SEND;
}
@ -132,32 +142,36 @@ ReturnValue_t RwHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand
return result;
}
// set speed flag.
commandBuffer[0] = true;
rawPacketLen = 1;
uint8_t* currentCmdBuf = commandBuffer + 1;
rwSpeedActuationSet.serialize(&currentCmdBuf, &rawPacketLen, sizeof(commandBuffer),
SerializeIF::Endianness::MACHINE);
commandBuffer[rawPacketLen++] = static_cast<uint8_t>(rws::SpecialRwRequest::REQUEST_NONE);
rawPacket = commandBuffer;
currentRequest.setSpeed = true;
rwSpeedActuationSet.getRwSpeed(currentRequest.currentRwSpeed, currentRequest.currentRampTime);
currentRequest.specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
rawPacketLen = sizeof(rws::RwRequest);
return returnvalue::OK;
}
case (rws::RESET_MCU): {
commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::RESET_MCU);
currentRequest.setSpeed = false;
currentRequest.specialRequest = rws::SpecialRwRequest::RESET_MCU;
internalState = InternalState::RESET_MCU;
rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
rawPacketLen = sizeof(rws::RwRequest);
return returnvalue::OK;
}
case (rws::INIT_RW_CONTROLLER): {
commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::INIT_RW_CONTROLLER);
currentRequest.setSpeed = false;
currentRequest.specialRequest = rws::SpecialRwRequest::INIT_RW_CONTROLLER;
internalState = InternalState::INIT_RW_CONTROLLER;
rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
rawPacketLen = sizeof(rws::RwRequest);
return returnvalue::OK;
}
case (rws::GET_TM): {
commandBuffer[0] = false;
commandBuffer[7] = static_cast<uint8_t>(rws::SpecialRwRequest::GET_TM);
currentRequest.setSpeed = false;
currentRequest.specialRequest = rws::SpecialRwRequest::GET_TM;
internalState = InternalState::GET_TM;
rawPacket = reinterpret_cast<uint8_t*>(&currentRequest);
rawPacketLen = sizeof(rws::RwRequest);
return returnvalue::OK;
}
default:
@ -185,6 +199,9 @@ ReturnValue_t RwHandler::scanForReply(const uint8_t* start, size_t remainingSize
*foundLen = remainingSize;
*foundId = rws::REPLY_ID;
}
if (internalState == InternalState::SHUTDOWN and shutdownState == ShutdownState::STOP_POLLING) {
shutdownState = ShutdownState::DONE;
}
return returnvalue::OK;
}
@ -394,9 +411,10 @@ void RwHandler::handleGetRwStatusReply(const uint8_t* packet) {
statusSet.setValidity(true, true);
if (internalState == InternalState::SHUTDOWN and std::abs(tmDataset.rwCurrSpeed.value) <= 2) {
if (internalState == InternalState::SHUTDOWN and std::abs(tmDataset.rwCurrSpeed.value) <= 2 and
shutdownState == ShutdownState::SET_SPEED_ZERO) {
// Finish transition to off.
commandExecuted = true;
shutdownState = ShutdownState::STOP_POLLING;
}
if (statusSet.state == rws::STATE_ERROR) {

8
mission/acs/RwHandler.h
View File

@ -75,8 +75,8 @@ class RwHandler : public DeviceHandlerBase {
GpioIF* gpioComIF = nullptr;
gpioId_t enableGpio = gpio::NO_GPIO;
bool debugMode = false;
bool commandExecuted = false;
Countdown offTransitionCountdown = Countdown(5000);
rws::RwRequest currentRequest;
rws::StatusSet statusSet;
rws::LastResetSatus lastResetStatusSet;
@ -90,6 +90,12 @@ class RwHandler : public DeviceHandlerBase {
PoolEntry<uint16_t> rampTime = PoolEntry<uint16_t>({10});
enum class InternalState { DEFAULT, GET_TM, INIT_RW_CONTROLLER, RESET_MCU, SHUTDOWN };
enum class ShutdownState {
NONE,
SET_SPEED_ZERO,
STOP_POLLING,
DONE,
} shutdownState = ShutdownState::NONE;
InternalState internalState = InternalState::DEFAULT;

10
mission/acs/rwHelpers.h
View File

@ -4,6 +4,7 @@
#include <fsfw/datapoollocal/LocalPoolVariable.h>
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <mission/acs/defs.h>
#include "eive/resultClassIds.h"
#include "events/subsystemIdRanges.h"
@ -36,6 +37,15 @@ enum class SpecialRwRequest : uint8_t {
NUM_REQUESTS
};
struct RwRequest {
acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
bool setSpeed = true;
int32_t currentRwSpeed = 0;
uint16_t currentRampTime = 0;
rws::SpecialRwRequest specialRequest = rws::SpecialRwRequest::REQUEST_NONE;
uint8_t rwIdx = 0;
};
static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER;
static const ReturnValue_t SPI_WRITE_FAILURE = MAKE_RETURN_CODE(0xB0);

2
tmtc

@ -1 +1 @@
Subproject commit 91a8a2e89519ac20d9ddabec2d8eaeb7707718d5
Subproject commit 98a9601dd7610dc7d2f47a622da898900c7e7f04