diff --git a/mission/devices/StarTrackerHandler.cpp b/mission/devices/StarTrackerHandler.cpp index c5718f02..22a84973 100644 --- a/mission/devices/StarTrackerHandler.cpp +++ b/mission/devices/StarTrackerHandler.cpp @@ -6,6 +6,7 @@ extern "C" { #include + #include #include "common/misc.h" } @@ -55,6 +56,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi const uint8_t * commandData, size_t commandDataLen) { switch (deviceCommand) { + case (StarTracker::PING_REQUEST): { + preparePingRequest(); + return RETURN_OK; + } case (StarTracker::REQ_TEMPERATURE): { prepareTemperatureRequest(); return RETURN_OK; @@ -68,6 +73,8 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi void StarTrackerHandler::fillCommandAndReplyMap() { /** Reply lengths are unknown because of the slip encoding. Thus always maximum reply size * is specified */ + this->insertInCommandAndReplyMap(StarTracker::PING_REQUEST, 1, nullptr, + StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::REQ_TEMPERATURE, 1, &temperatureSet, StarTracker::MAX_FRAME_SIZE * 2 + 2); } @@ -106,6 +113,11 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema } switch (decodedFrame[1]) { + case (static_cast(StarTracker::PING_REQUEST)): { + *foundLen = decodedLength; + *foundId = StarTracker::PING_REQUEST; + break; + } case (static_cast(StarTracker::REQ_TEMPERATURE)): { *foundLen = decodedLength; *foundId = StarTracker::REQ_TEMPERATURE; @@ -125,6 +137,10 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { + case (StarTracker::PING_REQUEST): { + handlePingReply(); + break; + } case (StarTracker::REQ_TEMPERATURE): { handleTemperatureTm(); break; @@ -170,6 +186,16 @@ void StarTrackerHandler::slipInit() { slipInfo.prev_state = SLIP_COMPLETE; } +void StarTrackerHandler::preparePingRequest() { + uint32_t length = 0; + struct PingActionRequest pingRequest = {PING_ID}; + arc_pack_ping_action_req(&pingRequest, 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); @@ -179,6 +205,17 @@ void StarTrackerHandler::prepareTemperatureRequest() { rawPacketLen = encLength; } +void StarTrackerHandler::handlePingReply() { + uint32_t pingId = 0; + size_t size = 0; + const uint8_t* buffer = decodedFrame; + SerializeAdapter::deSerialize(&pingId, &buffer, &size, SerializeIF::Endianness::BIG); +#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; +#endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ +} + void StarTrackerHandler::handleTemperatureTm() { PoolReadGuard rg(&temperatureSet); uint32_t offset = 1; diff --git a/mission/devices/StarTrackerHandler.h b/mission/devices/StarTrackerHandler.h index ee96e544..4d49064d 100644 --- a/mission/devices/StarTrackerHandler.h +++ b/mission/devices/StarTrackerHandler.h @@ -2,6 +2,7 @@ #define MISSION_DEVICES_STARTRACKERHANDLER_H_ #include +#include #include #include @@ -64,8 +65,11 @@ 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); - //! P1: TM id + + // Ping request will reply ping with this ID (data field) + static const uint32_t PING_ID = 0x55; StarTracker::TemperatureSet temperatureSet; @@ -89,8 +93,16 @@ private: */ void slipInit(); + /** + * @brief Fills command buffer with data to ping the star tracker + */ + void preparePingRequest(); + /** + * @brief Fills command buffer with data to request temperature from star tracker + */ void prepareTemperatureRequest(); + void handlePingReply(); /** * @brief This function handles the telemetry reply of a temperature request. */ diff --git a/mission/devices/devicedefinitions/StarTrackerDefinitions.h b/mission/devices/devicedefinitions/StarTrackerDefinitions.h index 6c28219b..efde795e 100644 --- a/mission/devices/devicedefinitions/StarTrackerDefinitions.h +++ b/mission/devices/devicedefinitions/StarTrackerDefinitions.h @@ -21,6 +21,7 @@ enum PoolIds: lp_id_t { +static const DeviceCommandId_t PING_REQUEST = 0; static const DeviceCommandId_t REQ_TEMPERATURE = 25; static const uint32_t TEMPERATURE_SET_ID = REQ_TEMPERATURE;