From 8b97528afb9a9975ad82fafa5d7c21cb750d4d9e Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 26 Nov 2021 13:16:05 +0100 Subject: [PATCH] star tracker ping and reboot command --- mission/devices/StarTrackerHandler.cpp | 90 ++++++++++++------- mission/devices/StarTrackerHandler.h | 18 ++-- .../StarTrackerDefinitions.h | 7 +- 3 files changed, 75 insertions(+), 40 deletions(-) diff --git a/mission/devices/StarTrackerHandler.cpp b/mission/devices/StarTrackerHandler.cpp index 22a84973..ebb23033 100644 --- a/mission/devices/StarTrackerHandler.cpp +++ b/mission/devices/StarTrackerHandler.cpp @@ -25,7 +25,8 @@ StarTrackerHandler::~StarTrackerHandler() { void StarTrackerHandler::doStartUp() { #if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 - setMode(MODE_NORMAL); +// setMode(MODE_NORMAL); + setMode(_MODE_TO_ON); #else setMode(_MODE_TO_ON); #endif @@ -60,6 +61,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi preparePingRequest(); return RETURN_OK; } + case (StarTracker::REBOOT): { + prepareRebootCommand(); + return RETURN_OK; + } case (StarTracker::REQ_TEMPERATURE): { prepareTemperatureRequest(); return RETURN_OK; @@ -75,6 +80,7 @@ void StarTrackerHandler::fillCommandAndReplyMap() { * is specified */ this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 1, nullptr, StarTracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandMap(StarTracker::REBOOT); this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 1, &temperatureSet, StarTracker::MAX_FRAME_SIZE * 2 + 2); } @@ -136,9 +142,11 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + ReturnValue_t result = RETURN_OK; + switch (id) { case (StarTracker::PING_REQUEST): { - handlePingReply(); + result = handlePingReply(); break; } case (StarTracker::REQ_TEMPERATURE): { @@ -146,12 +154,13 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con break; } default: { - sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id" << std::endl; + sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id + << std::endl; return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } } - return RETURN_OK; + return result; } void StarTrackerHandler::setNormalDatapoolEntriesInvalid() { @@ -165,7 +174,6 @@ uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - localDataPoolMap.emplace(StarTracker::STATUS, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::TICKS, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::TIME, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry( { 0 })); @@ -196,6 +204,16 @@ void StarTrackerHandler::preparePingRequest() { rawPacketLen = encLength; } +void StarTrackerHandler::prepareRebootCommand() { + uint32_t length = 0; + struct RebootActionRequest rebootReq; + arc_pack_reboot_action_req(&rebootReq, commandBuffer, &length); + uint32_t encLength = 0; + arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength); + rawPacket = encBuffer; + rawPacketLen = encLength; +} + void StarTrackerHandler::prepareTemperatureRequest() { uint32_t length = 0; arc_tm_pack_temperature_req(commandBuffer, &length); @@ -205,49 +223,59 @@ void StarTrackerHandler::prepareTemperatureRequest() { rawPacketLen = encLength; } -void StarTrackerHandler::handlePingReply() { +ReturnValue_t StarTrackerHandler::handlePingReply() { + ReturnValue_t result = RETURN_OK; uint32_t pingId = 0; - size_t size = 0; - const uint8_t* buffer = decodedFrame; - SerializeAdapter::deSerialize(&pingId, &buffer, &size, SerializeIF::Endianness::BIG); + uint8_t status = *(decodedFrame + 2); + const uint8_t* buffer = decodedFrame + 3; + 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(*(decodedFrame + 2)) << std::endl; - sif::info << "Ping id "<< pingId << std::endl; + sif::info << "Ping status: "<< static_cast(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) { + result = PING_FAILED; + } + return result; } -void StarTrackerHandler::handleTemperatureTm() { +ReturnValue_t StarTrackerHandler::handleTemperatureTm() { + ReturnValue_t result = RETURN_OK; PoolReadGuard rg(&temperatureSet); - uint32_t offset = 1; - temperatureSet.status = *(decodedFrame + offset); + uint32_t offset = 2; + uint8_t status = *(decodedFrame + offset); offset += 1; - if(temperatureSet.status.value != 0) { + if(status != StarTracker::STATUS_OK) { sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: " - << static_cast(temperatureSet.status.value) << std::endl; - triggerEvent(TM_REPLY_ERROR, temperatureSet.status.value); + << static_cast(status) << std::endl; + result = TEMPERATURE_REQUEST_FAILED; + return result; } - temperatureSet.ticks = *(decodedFrame + offset) << 24 - | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8 - | *(decodedFrame + offset + 3); - offset += 4; - temperatureSet.time = static_cast(*(decodedFrame + offset)) << 56 - | static_cast(*(decodedFrame + offset + 1)) << 48 - | static_cast(*(decodedFrame + offset + 2)) << 40 - | static_cast(*(decodedFrame + offset + 3)) << 32 - | *(decodedFrame + offset + 4) << 24 | *(decodedFrame + offset + 5) << 16 - | *(decodedFrame + offset + 6) << 8 | *(decodedFrame + offset + 7); - offset += 8; + const uint8_t* buffer = decodedFrame + offset; + size_t size = sizeof(temperatureSet.ticks); + SerializeAdapter::deSerialize(&temperatureSet.ticks, &buffer, &size, SerializeIF::Endianness::LITTLE); + offset += size; + buffer = decodedFrame + offset; + size = sizeof(temperatureSet.time); + SerializeAdapter::deSerialize(&temperatureSet.time, &buffer, &size, SerializeIF::Endianness::LITTLE); + offset += size; temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24 | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8 | *(decodedFrame + offset + 3); - offset += 4; - temperatureSet.cmosTemperature = *(decodedFrame + offset) << 24 + offset += sizeof(temperatureSet.mcuTemperature); + temperatureSet.mcuTemperature = *(decodedFrame + offset) << 24 | *(decodedFrame + offset + 1) << 16 | *(decodedFrame + offset + 2) << 8 | *(decodedFrame + offset + 3); -#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 + #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 + sif::info << "StarTrackerHandler::handleTemperatureTm: Ticks: " + << temperatureSet.ticks << std::endl; + sif::info << "StarTrackerHandler::handleTemperatureTm: Time: " + << temperatureSet.time << std::endl; sif::info << "StarTrackerHandler::handleTemperatureTm: MCU Temperature: " << temperatureSet.mcuTemperature << " °C" << std::endl; sif::info << "StarTrackerHandler::handleTemperatureTm: CMOS Temperature: " << temperatureSet.mcuTemperature << " °C" << std::endl; #endif + return result; } diff --git a/mission/devices/StarTrackerHandler.h b/mission/devices/StarTrackerHandler.h index 4d49064d..329536dd 100644 --- a/mission/devices/StarTrackerHandler.h +++ b/mission/devices/StarTrackerHandler.h @@ -12,7 +12,6 @@ * @details Datasheet: https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/ * Arbeitsdaten/08_Used%20Components/ArcSec_KULeuven_Startracker/ * Sagitta%201.0%20Datapack&fileid=659181 - * * @author J. Meier */ class StarTrackerHandler: public DeviceHandlerBase { @@ -64,9 +63,10 @@ private: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HANDLER; - //! [EXPORT] : [COMMENT] Result code of tm reply indicates an error - - static const ReturnValue_t TM_REPLY_ERROR = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] Status in tm reply not ok + static const ReturnValue_t TEMPERATURE_REQUEST_FAILED = MAKE_RETURN_CODE(0xA0); + //! [EXPORT] : [COMMENT] Ping command failed + static const ReturnValue_t PING_FAILED = MAKE_RETURN_CODE(0xA1); // Ping request will reply ping with this ID (data field) static const uint32_t PING_ID = 0x55; @@ -97,16 +97,22 @@ private: * @brief Fills command buffer with data to ping the star tracker */ void preparePingRequest(); + + /** + * @brief Fills command buffer with data to reboot star tracker. + */ + void prepareRebootCommand(); + /** * @brief Fills command buffer with data to request temperature from star tracker */ void prepareTemperatureRequest(); - void handlePingReply(); + ReturnValue_t handlePingReply(); /** * @brief This function handles the telemetry reply of a temperature request. */ - void handleTemperatureTm(); + ReturnValue_t handleTemperatureTm(); }; #endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/StarTrackerDefinitions.h b/mission/devices/devicedefinitions/StarTrackerDefinitions.h index efde795e..0b30b661 100644 --- a/mission/devices/devicedefinitions/StarTrackerDefinitions.h +++ b/mission/devices/devicedefinitions/StarTrackerDefinitions.h @@ -11,8 +11,9 @@ namespace StarTracker { /** This is the address of the star tracker */ static const uint8_t ADDRESS = 33; +static const uint8_t STATUS_OK = 0; + enum PoolIds: lp_id_t { - STATUS, TICKS, TIME, MCU_TEMPERATURE, @@ -22,6 +23,7 @@ enum PoolIds: lp_id_t { static const DeviceCommandId_t PING_REQUEST = 0; +static const DeviceCommandId_t REBOOT = 7; static const DeviceCommandId_t REQ_TEMPERATURE = 25; static const uint32_t TEMPERATURE_SET_ID = REQ_TEMPERATURE; @@ -46,8 +48,7 @@ public: StaticLocalDataSet(sid_t(objectId, TEMPERATURE_SET_ID)) { } - lp_var_t status = lp_var_t(sid.objectId, - PoolIds::STATUS, this); + // Ticks is time reference generated by interanl counter of the star tracker lp_var_t ticks = lp_var_t(sid.objectId, PoolIds::TICKS, this); /** Unix time in microseconds */