From eb0e9c2a4193d871807257e64f1dd938b387ac72 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 26 Nov 2021 15:24:52 +0100 Subject: [PATCH] request version command --- mission/devices/StarTrackerHandler.cpp | 65 +++++++++++++++++-- mission/devices/StarTrackerHandler.h | 11 ++++ .../StarTrackerDefinitions.h | 49 ++++++++++++-- 3 files changed, 115 insertions(+), 10 deletions(-) diff --git a/mission/devices/StarTrackerHandler.cpp b/mission/devices/StarTrackerHandler.cpp index ebb23033..10805f7e 100644 --- a/mission/devices/StarTrackerHandler.cpp +++ b/mission/devices/StarTrackerHandler.cpp @@ -12,7 +12,7 @@ extern "C" { StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) : - DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this) { + DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this) { if (comCookie == NULL) { sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl; } @@ -61,6 +61,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi preparePingRequest(); return RETURN_OK; } + case (StarTracker::REQ_VERSION): { + prepareVersionRequest(); + return RETURN_OK; + } case (StarTracker::REBOOT): { prepareRebootCommand(); return RETURN_OK; @@ -80,6 +84,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() { * is specified */ this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 1, nullptr, StarTracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(StarTracker::REQ_VERSION, 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); @@ -124,6 +130,11 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema *foundId = StarTracker::PING_REQUEST; break; } + case (static_cast(StarTracker::REQ_VERSION)): { + *foundLen = decodedLength; + *foundId = StarTracker::PING_REQUEST; + break; + } case (static_cast(StarTracker::REQ_TEMPERATURE)): { *foundLen = decodedLength; *foundId = StarTracker::REQ_TEMPERATURE; @@ -149,6 +160,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con result = handlePingReply(); break; } + case (StarTracker::REQ_VERSION): { + result = handleVersionTm(); + break; + } case (StarTracker::REQ_TEMPERATURE): { handleTemperatureTm(); break; @@ -174,8 +189,13 @@ uint32_t StarTrackerHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - localDataPoolMap.emplace(StarTracker::TICKS, new PoolEntry( { 0 })); - localDataPoolMap.emplace(StarTracker::TIME, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TICKS_VERSION_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TIME_VERSION_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::PROGRAM, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::MAJOR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::MINOR, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TICKS_TEMPERATURE_SET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::TIME_TEMPERATURE_SET, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::MCU_TEMPERATURE, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::CMOS_TEMPERATURE, new PoolEntry( { 0 })); @@ -204,6 +224,15 @@ void StarTrackerHandler::preparePingRequest() { rawPacketLen = encLength; } +void StarTrackerHandler::prepareVersionRequest() { + uint32_t length = 0; + arc_tm_pack_version_req(commandBuffer, &length); + uint32_t encLength = 0; + arc_transport_encode_body(commandBuffer, length, encBuffer, &encLength); + rawPacket = encBuffer; + rawPacketLen = encLength; +} + void StarTrackerHandler::prepareRebootCommand() { uint32_t length = 0; struct RebootActionRequest rebootReq; @@ -240,6 +269,34 @@ ReturnValue_t StarTrackerHandler::handlePingReply() { return result; } +ReturnValue_t StarTrackerHandler::handleVersionTm() { + ReturnValue_t result = RETURN_OK; + PoolReadGuard rg(&versionSet); + uint32_t offset = 2; + uint8_t status = *(decodedFrame + offset); + offset += 1; + if(status != StarTracker::STATUS_OK) { + sif::warning << "StarTrackerHandler::handleTemperatureTm: Reply error: " + << static_cast(status) << std::endl; + result = TEMPERATURE_REQUEST_FAILED; + return result; + } + versionSet.program = (*decodedFrame + offset); + offset += 1; + versionSet.major = (*decodedFrame + offset); + offset += 1; + versionSet.minor = (*decodedFrame + offset); +#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 + sif::info << "StarTrackerHandler::handleVersionTm: Program: " + << versionSet.program << std::endl; + sif::info << "StarTrackerHandler::handleVersionTm: Major: " + << versionSet.major << std::endl; + sif::info << "StarTrackerHandler::handleVersionTm: Minor: " + << versionSet.minor << std::endl; +#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ + return result; +} + ReturnValue_t StarTrackerHandler::handleTemperatureTm() { ReturnValue_t result = RETURN_OK; PoolReadGuard rg(&temperatureSet); @@ -267,7 +324,7 @@ ReturnValue_t StarTrackerHandler::handleTemperatureTm() { 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: " diff --git a/mission/devices/StarTrackerHandler.h b/mission/devices/StarTrackerHandler.h index 329536dd..89bd7097 100644 --- a/mission/devices/StarTrackerHandler.h +++ b/mission/devices/StarTrackerHandler.h @@ -72,6 +72,7 @@ private: static const uint32_t PING_ID = 0x55; StarTracker::TemperatureSet temperatureSet; + StarTracker::VersionSet versionSet; uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE]; uint8_t rxBuffer[StarTracker::MAX_FRAME_SIZE]; @@ -98,6 +99,11 @@ private: */ void preparePingRequest(); + /** + * @brief Fills command buffer with data to request the version telemetry packet + */ + void prepareVersionRequest(); + /** * @brief Fills command buffer with data to reboot star tracker. */ @@ -113,6 +119,11 @@ private: * @brief This function handles the telemetry reply of a temperature request. */ ReturnValue_t handleTemperatureTm(); + + /** + * @brief This function handles the telemetry reply of a version request. + */ + ReturnValue_t handleVersionTm(); }; #endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/StarTrackerDefinitions.h b/mission/devices/devicedefinitions/StarTrackerDefinitions.h index 0b30b661..6772abfe 100644 --- a/mission/devices/devicedefinitions/StarTrackerDefinitions.h +++ b/mission/devices/devicedefinitions/StarTrackerDefinitions.h @@ -14,24 +14,32 @@ static const uint8_t ADDRESS = 33; static const uint8_t STATUS_OK = 0; enum PoolIds: lp_id_t { - TICKS, - TIME, + TICKS_VERSION_SET, + TIME_VERSION_SET, + TICKS_TEMPERATURE_SET, + TIME_TEMPERATURE_SET, MCU_TEMPERATURE, - CMOS_TEMPERATURE + CMOS_TEMPERATURE, + PROGRAM, + MAJOR, + MINOR }; static const DeviceCommandId_t PING_REQUEST = 0; +static const DeviceCommandId_t REQ_VERSION = 2; static const DeviceCommandId_t REBOOT = 7; static const DeviceCommandId_t REQ_TEMPERATURE = 25; +static const uint32_t VERSION_SET_ID = REQ_VERSION; static const uint32_t TEMPERATURE_SET_ID = REQ_TEMPERATURE; /** Max size of unencoded frame */ static const size_t MAX_FRAME_SIZE = 1200; -static const uint8_t TEMPERATURE_SET_ENTRIES = 5; +static const uint8_t TEMPERATURE_SET_ENTRIES = 4; +static const uint8_t VERSION_SET_ENTRIES = 3; /** * @brief This dataset can be used to store the temperature of a reaction wheel. @@ -50,16 +58,45 @@ public: // Ticks is time reference generated by interanl counter of the star tracker lp_var_t ticks = lp_var_t(sid.objectId, - PoolIds::TICKS, this); + PoolIds::TICKS_TEMPERATURE_SET, this); /** Unix time in microseconds */ lp_var_t time = lp_var_t(sid.objectId, - PoolIds::TIME, this); + PoolIds::TIME_TEMPERATURE_SET, this); lp_var_t mcuTemperature = lp_var_t(sid.objectId, PoolIds::MCU_TEMPERATURE, this); lp_var_t cmosTemperature = lp_var_t(sid.objectId, PoolIds::CMOS_TEMPERATURE, this); }; +/** + * @brief Package to store version parameters + */ +class VersionSet: + public StaticLocalDataSet { +public: + + VersionSet(HasLocalDataPoolIF* owner): + StaticLocalDataSet(owner, VERSION_SET_ID) { + } + + VersionSet(object_id_t objectId): + StaticLocalDataSet(sid_t(objectId, VERSION_SET_ID)) { + } + + // Ticks is time reference generated by interanl counter of the star tracker + lp_var_t ticks = lp_var_t(sid.objectId, + PoolIds::TICKS_VERSION_SET, this); + /** Unix time in microseconds */ + lp_var_t time = lp_var_t(sid.objectId, + PoolIds::TIME_VERSION_SET, this); + lp_var_t program = lp_var_t(sid.objectId, + PoolIds::PROGRAM, this); + lp_var_t major = lp_var_t(sid.objectId, + PoolIds::MAJOR, this); + lp_var_t minor = lp_var_t(sid.objectId, + PoolIds::MINOR, this); +}; + } #endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */