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

star tracker improvements

Browse Source
This commit is contained in:
Jakob Meier 2021-12-14 19:24:31 +01:00
parent 74f52632f6
commit 717f2e21a0
6 changed files with 278 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
bsp_q7s/devices/startracker/StarTrackerDefinitions.h
View File

@ -92,7 +92,7 @@ static const DeviceCommandId_t REQ_INTERFACE = 3;
static const DeviceCommandId_t REQ_TIME = 4;
static const DeviceCommandId_t ERASE = 5;
static const DeviceCommandId_t UNLOCK = 6;
static const DeviceCommandId_t REBOOT = 7;
static const DeviceCommandId_t SWITCH_TO_BOOTLOADER_PROGRAM = 7;
static const DeviceCommandId_t DOWNLOAD_IMAGE = 9;
static const DeviceCommandId_t UPLOAD_IMAGE = 10;
static const DeviceCommandId_t REQ_POWER = 11;
@ -120,6 +120,7 @@ static const DeviceCommandId_t DOWNLOAD_DB_IMAGE = 54;
static const DeviceCommandId_t STOP_IMAGE_LOADER = 55;
static const DeviceCommandId_t RESET_ERROR = 56;
static const DeviceCommandId_t CHANGE_DOWNLOAD_FILE = 57;
static const DeviceCommandId_t SET_JSON_FILE_NAME = 58;
static const DeviceCommandId_t NONE = 0xFFFFFFFF;
static const uint32_t VERSION_SET_ID = REQ_VERSION;
@ -171,6 +172,11 @@ namespace ID {
static const uint8_t ERROR_RESET = 12;
}
namespace Program {
static const uint8_t BOOTLOADER = 1;
static const uint8_t FIRMWARE = 2;
}
/**
* @brief This dataset can be used to store the temperature of a reaction wheel.
*/

224
bsp_q7s/devices/startracker/StarTrackerHandler.cpp
View File

@ -81,9 +81,19 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
if (imageLoaderExecuting == true) {
return IMAGE_LOADER_EXECUTING;
}
result = checkMode(actionId);
if (result != RETURN_OK) {
return result;
}
// Intercept image loader commands which do not follow the common DHB communication flow
switch(actionId) {
case(StarTracker::UPLOAD_IMAGE): {
result = DeviceHandlerBase::acceptExternalDeviceCommands();
if (result != RETURN_OK) {
return result;
}
if (size > MAX_PATH_SIZE) {
return FILE_PATH_TOO_LONG;
}
@ -96,6 +106,10 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu
return EXECUTION_FINISHED;
}
case(StarTracker::DOWNLOAD_IMAGE): {
result = DeviceHandlerBase::acceptExternalDeviceCommands();
if (result != RETURN_OK) {
return result;
}
if (size > MAX_PATH_SIZE) {
return FILE_PATH_TOO_LONG;
}
@ -138,19 +152,33 @@ void StarTrackerHandler::performOperationHook() {
}
void StarTrackerHandler::doStartUp() {
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
// setMode(MODE_NORMAL);
switch(startupState) {
case StartupState::IDLE:
startupState = StartupState::CHECK_BOOT_STATE;
return;
case StartupState::BOOT_DELAY:
if (bootCountdown.hasTimedOut()) {
startupState = StartupState::LIMITS;
}
return;
case StartupState::DONE:
break;
default:
return;
}
setMode(_MODE_TO_ON);
#else
setMode(_MODE_TO_ON);
#endif
}
void StarTrackerHandler::doShutDown() {
// If star tracker is shutdown also stop all running processes in the image loader task
strImageLoader->stopProcess();
setMode(_MODE_POWER_DOWN);
}
void StarTrackerHandler::doOffActivity() {
startupState = StartupState::IDLE;
}
ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) {
switch (internalState) {
case InternalState::TEMPERATURE_REQUEST:
@ -165,6 +193,72 @@ ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * i
}
ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) {
if (mode != _MODE_START_UP) {
return NOTHING_TO_SEND;
}
switch (startupState) {
case StartupState::CHECK_BOOT_STATE:
*id = StarTracker::REQ_VERSION;
startupState = StartupState::WAIT_FOR_EXECUTION;
return buildCommandFromCommand(*id, nullptr, 0);
case StartupState::BOOT:
*id = StarTracker::BOOT;
bootCountdown.setTimeout(BOOT_TIMEOUT);
startupState = StartupState::BOOT_DELAY;
return buildCommandFromCommand(*id, nullptr, 0);
case StartupState::LIMITS:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::LIMITS;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::TRACKING:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::TRACKING;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::MOUNTING:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::MOUNTING;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::CAMERA:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::CAMERA;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::BLOB:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::BLOB;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::CENTROIDING:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::CENTROIDING;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::LISA:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::LISA;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::MATCHING:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::MATCHING;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::VALIDATION:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::VALIDATION;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
case StartupState::ALGO:
startupState = StartupState::WAIT_FOR_EXECUTION;
*id = StarTracker::ALGO;
return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
paramJsonFile.size());
default:
break;
}
return NOTHING_TO_SEND;
}
@ -196,7 +290,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi
preparePowerRequest();
return RETURN_OK;
}
case (StarTracker::REBOOT): {
case (StarTracker::SWITCH_TO_BOOTLOADER_PROGRAM): {
prepareRebootCommand();
return RETURN_OK;
}
@ -293,7 +387,7 @@ void StarTrackerHandler::fillCommandAndReplyMap() {
this->insertInCommandAndReplyMap(StarTracker::REQ_INTERFACE, 3, &interfaceSet,
StarTracker::MAX_FRAME_SIZE * 2 + 2);
// Reboot has no reply. Star tracker reboots immediately
this->insertInCommandMap(StarTracker::REBOOT);
this->insertInCommandMap(StarTracker::SWITCH_TO_BOOTLOADER_PROGRAM);
this->insertInCommandAndReplyMap(StarTracker::SUBSCRIBE_TO_TM, 3, nullptr,
StarTracker::MAX_FRAME_SIZE * 2 + 2);
this->insertInCommandAndReplyMap(StarTracker::REQ_SOLUTION, 3, &solutionSet,
@ -406,6 +500,13 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con
}
case (StarTracker::REQ_VERSION): {
result = handleVersionTm();
if (result != RETURN_OK) {
return result;
}
result = checkProgram();
if (result != RETURN_OK) {
return result;
}
break;
}
case (StarTracker::REQ_INTERFACE): {
@ -535,21 +636,29 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l
}
size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId){
// Prevent DHB from polling UART during upload command. Because UART is used by image loader
// task
if (commandId == StarTracker::UPLOAD_IMAGE) {
return 0;
}
return StarTracker::MAX_FRAME_SIZE;
}
ReturnValue_t StarTrackerHandler::doSendReadHook() {
// Prevent DHB from polling UART during commands executed by the image loader task
if(imageLoaderExecuting) {
return RETURN_FAILED;
}
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) {
switch(actionId) {
case StarTracker::UPLOAD_IMAGE:
case StarTracker::DOWNLOAD_IMAGE: {
return DeviceHandlerBase::acceptExternalDeviceCommands();
default:
break;
}
}
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) {
const uint8_t* reply = dataLinkLayer.getReply();
switch (*reply) {
@ -836,8 +945,14 @@ ReturnValue_t StarTrackerHandler::handleSetParamReply() {
sif::warning << "StarTrackerHandler::handleSetParamReply: Failed to execute parameter set "
" command with parameter ID" <<
static_cast<unsigned int>(*(reply + PARAMETER_ID_OFFSET)) << std::endl;
if (startupState != StartupState::IDLE) {
startupState = StartupState::IDLE;
}
return SET_PARAM_FAILED;
}
if (startupState != StartupState::IDLE) {
handleStartup(reply + PARAMETER_ID_OFFSET);
}
return RETURN_OK;
}
@ -863,13 +978,18 @@ ReturnValue_t StarTrackerHandler::handlePingReply() {
size_t size = sizeof(pingId);
SerializeAdapter::deSerialize(&pingId, &buffer, &size, SerializeIF::Endianness::LITTLE);
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
sif::info << "Ping status: "<< static_cast<unsigned int>(status) << std::endl;
sif::info << "StarTracker: Ping status: "<< static_cast<unsigned int>(status) << std::endl;
sif::info << "Ping id: 0x"<< std::hex << pingId << std::endl;
#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */
if (status != StarTracker::STATUS_OK || pingId != PING_ID) {
sif::warning << "StarTrackerHandler::handlePingReply: Ping failed" << std::endl;
result = PING_FAILED;
}
else {
#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1
sif::info << "StarTracker: Ping successful" << std::endl;
#endif
}
return result;
}
@ -885,7 +1005,7 @@ ReturnValue_t StarTrackerHandler::handleTimeTm() {
uint64_t time = 0;
getTmHeaderData(&status, &ticks, &time);
if(status != StarTracker::STATUS_OK) {
sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: "
sif::warning << "StarTrackerHandler::handleTimeTm: Reply error: "
<< static_cast<unsigned int>(status) << std::endl;
result = VERSION_REQ_FAILED;
return result;
@ -941,6 +1061,30 @@ ReturnValue_t StarTrackerHandler::handleVersionTm() {
return result;
}
ReturnValue_t StarTrackerHandler::checkProgram() {
PoolReadGuard pg(&versionSet);
switch(versionSet.program.value) {
case StarTracker::Program::BOOTLOADER:
// Star tracker currently in bootloader program. Need to send boot command to switch to
// firmware program
if (startupState != StartupState::IDLE) {
startupState = StartupState::BOOT;
}
break;
case StarTracker::Program::FIRMWARE:
// Firmware already booted
if (startupState != StartupState::IDLE) {
startupState = StartupState::LIMITS;
}
break;
default:
sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID"
<< std::endl;
return INVALID_PROGRAM;
}
return RETURN_OK;
}
ReturnValue_t StarTrackerHandler::handleInterfaceTm() {
ReturnValue_t result = RETURN_OK;
uint32_t offset = TM_DATA_FIELD_OFFSET;
@ -1203,3 +1347,53 @@ uint64_t StarTrackerHandler::deserializeUint64(const uint8_t* buffer) {
| static_cast<uint64_t>(*(buffer));
return word;
}
void StarTrackerHandler::handleStartup(const uint8_t* parameterId) {
switch(*parameterId) {
case (StarTracker::ID::LIMITS): {
startupState = StartupState::TRACKING;
break;
}
case (StarTracker::ID::TRACKING): {
startupState = StartupState::MOUNTING;
break;
}
case (StarTracker::ID::MOUNTING): {
startupState = StartupState::CAMERA;
break;
}
case (StarTracker::ID::CAMERA): {
startupState = StartupState::BLOB;
break;
}
case (StarTracker::ID::BLOB): {
startupState = StartupState::CENTROIDING;
break;
}
case (StarTracker::ID::CENTROIDING): {
startupState = StartupState::LISA;
break;
}
case (StarTracker::ID::LISA): {
startupState = StartupState::MATCHING;
break;
}
case (StarTracker::ID::MATCHING): {
startupState = StartupState::VALIDATION;
break;
}
case (StarTracker::ID::VALIDATION): {
startupState = StartupState::ALGO;
break;
}
case (StarTracker::ID::ALGO): {
startupState = StartupState::DONE;
break;
}
default: {
sif::debug << "StarTrackerHandler::handleStartup: Received parameter reply with unexpected"
<< " parameter ID" << std::endl;
break;
}
}
}

57
bsp_q7s/devices/startracker/StarTrackerHandler.h
View File

@ -3,6 +3,7 @@
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "fsfw/src/fsfw/serialize/SerializeAdapter.h"
#include "fsfw/timemanager/Countdown.h"
#include "thirdparty/arcsec_star_tracker/common/SLIP.h"
#include <fsfw/datapool/PoolReadGuard.h>
#include "StrImageLoader.h"
@ -49,6 +50,7 @@ public:
protected:
void doStartUp() override;
void doShutDown() override;
void doOffActivity() override;
ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override;
ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override;
void fillCommandAndReplyMap() override;
@ -62,12 +64,10 @@ protected:
uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
LocalDataPoolManager& poolManager) override;
/**
* @brief Overwritten here to always read all available data from the UartComIF.
*/
virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override;
virtual ReturnValue_t doSendReadHook() override;
private:
@ -109,6 +109,8 @@ private:
static const ReturnValue_t FILE_PATH_TOO_LONG = MAKE_RETURN_CODE(0xAB);
//! [EXPORT] : [COMMENT] Name of file received with command is too long
static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xAC);
//! [EXPORT] : [COMMENT] Received version reply with invalid program ID
static const ReturnValue_t INVALID_PROGRAM = MAKE_RETURN_CODE(0xAD);
static const size_t MAX_PATH_SIZE = 50;
static const size_t MAX_FILE_NAME = 30;
@ -124,13 +126,12 @@ private:
static const uint8_t PARAMETER_ID_OFFSET = 0;
static const uint8_t ACTION_ID_OFFSET = 0;
static const uint8_t ACTION_DATA_OFFSET = 2;
// Ping request will reply ping with this ID (data field)
static const uint32_t PING_ID = 0x55;
static const uint32_t BOOT_REGION_ID = 1;
static const MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
static const uint32_t MUTEX_TIMEOUT = 20;
static const uint32_t BOOT_TIMEOUT = 1000;
MessageQueueIF* eventQueue = nullptr;
@ -148,14 +149,52 @@ private:
uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE];
// Countdown to insert delay for star tracker to switch from bootloader to firmware program
Countdown bootCountdown;
std::string paramJsonFile = "/mnt/sd0/startracker/full.json";
enum class InternalState {
TEMPERATURE_REQUEST
};
InternalState internalState = InternalState::TEMPERATURE_REQUEST;
enum class StartupState {
IDLE,
CHECK_BOOT_STATE,
BOOT,
BOOT_DELAY,
LIMITS,
TRACKING,
MOUNTING,
CAMERA,
BLOB,
CENTROIDING,
LISA,
MATCHING,
VALIDATION,
ALGO,
WAIT_FOR_EXECUTION,
DONE
};
StartupState startupState = StartupState::IDLE;
bool imageLoaderExecuting = false;
/**
* @brief Handles internal state
*/
void handleInternalState();
/**
* @brief Checks mode for commands requiring MODE_ON of MODE_NORMAL for execution.
*
* @param actionId Action id of command to execute
*/
ReturnValue_t checkMode(ActionId_t actionId);
/**
* @brief This function initializes the serial link ip protocol struct slipInfo.
*/
@ -279,6 +318,11 @@ private:
*/
ReturnValue_t handleVersionTm();
/**
* @brief Checks the loaded program by means of the version set
*/
ReturnValue_t checkProgram();
/**
* @brief Handles reply to request interface telemetry command.
*/
@ -315,6 +359,11 @@ private:
* @note Deserialization will be performed in little endian byte order
*/
uint64_t deserializeUint64(const uint8_t* buffer);
/**
* @brief Handles the startup state machine
*/
void handleStartup(const uint8_t* parameterId);
};
#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */

4
bsp_q7s/devices/startracker/StrImageLoader.cpp
View File

@ -272,7 +272,7 @@ ReturnValue_t StrImageLoader::sendAndRead(size_t size, uint32_t position) {
ReturnValue_t StrImageLoader::checkReply() {
uint8_t type = datalinkLayer.getReplyFrameType();
if (type != TMTC_ACTIONREPLY) {
sif::warning << "StrImageLoader::checkUploadReply: Received reply with invalid type ID"
sif::warning << "StrImageLoader::checkReply: Received reply with invalid type ID"
<< std::endl;
triggerEvent(INVALID_TYPE_ID);
return RETURN_FAILED;
@ -280,7 +280,7 @@ ReturnValue_t StrImageLoader::checkReply() {
uint8_t status = datalinkLayer.getStatusField();
if (status != ArcsecDatalinkLayer::STATUS_OK) {
triggerEvent(STATUS_ERROR);
sif::warning << "StrImageLoader::checkUploadReply: Status failure" << std::endl;
sif::warning << "StrImageLoader::checkReply: Status failure" << std::endl;
return RETURN_FAILED;
}
return RETURN_OK;

7
bsp_q7s/devices/startracker/StrImageLoader.h
View File

@ -170,7 +170,12 @@ private:
ReturnValue_t performImageUpload();
/**
* @bried Performs download of last taken image from the star tracker.
* @brief Performs download of last taken image from the star tracker.
*
* @details Download is split over multiple packets transporting each a maximum of 1024 bytes.
* In case the download of one position fails, the same packet will be again
* requested. If the download of the packet fails CONFIG_MAX_DOWNLOAD_RETRIES times,
* the download will be stopped.
*/
ReturnValue_t performImageDownload();

1
linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp
View File

@ -568,7 +568,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) {
ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) {
// Length of a communication cycle
uint32_t length = thisSequence->getPeriodMs();
static_cast<void>(length);
bool uartPstEmpty = true;
#if OBSW_ADD_PLOC_MPSOC == 1