diff --git a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h index eec460e3..8a58149e 100644 --- a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h +++ b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h @@ -196,7 +196,22 @@ enum PoolIds: lp_id_t { BLOBPIX_TOT_VAL, BLOBPIX_IN_USE, BLOBPIX_BRIGHT_NEIGHBOURS, - BLOBPIX_REGION + BLOBPIX_REGION, + CAMERA_MODE, + FOCALLENGTH, + EXPOSURE, + INTERVAL, + CAMERA_OFFSET, + PGAGAIN, + ADCGAIN, + CAM_REG1, + CAM_VAL1, + CAM_REG2, + CAM_VAL2, + CAM_REG3, + CAM_VAL3, + CAM_REG4, + CAM_VAL4 }; static const DeviceCommandId_t PING_REQUEST = 0; @@ -245,6 +260,7 @@ static const DeviceCommandId_t DOWNLOAD_FPGA_IMAGE = 63; static const DeviceCommandId_t CHANGE_FPGA_DOWNLOAD_FILE = 64; static const DeviceCommandId_t UPLOAD_FPGA_IMAGE = 65; static const DeviceCommandId_t FPGA_ACTION = 66; +static const DeviceCommandId_t REQ_CAMERA_PARAMS = 67; static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; @@ -260,6 +276,7 @@ static const uint32_t DOWNLOADCENTROID_SET_ID = DOWNLOAD_CENTROID; static const uint32_t DOWNLOAD_MATCHED_STAR_SET_ID = DOWNLOAD_MATCHED_STAR; static const uint32_t DOWNLOAD_DBIMAGE_SET_ID = DOWNLOAD_DBIMAGE; static const uint32_t DOWNLOAD_BLOBPIXEL_SET_ID = DOWNLOAD_BLOBPIXEL; +static const uint32_t CAMERA_SET_ID = REQ_CAMERA_PARAMS; /** Max size of unencoded frame */ static const size_t MAX_FRAME_SIZE = 1200; @@ -277,6 +294,7 @@ static const uint8_t DOWNLOAD_CENTROID_SET_ENTRIES = 11; static const uint8_t DOWNLOAD_MATCHED_STAR_SET_ENTRIES = 14; static const uint8_t DOWNLOAD_DBIMAGE_SET_ENTRIES = 6; static const uint8_t DOWNLOAD_BLOBPIXEL_SET_ENTRIES = 7; +static const uint8_t CAMERA_SET_ENTRIES = 15; // Action, parameter and telemetry IDs namespace ID { @@ -1206,5 +1224,70 @@ public: sif::info << "DownloadBlobPixel::printSet: region: " << this->region << std::endl; } }; + +/** + * @brief Will store the camera parameters set in the star tracker which are retrieved with + * a get parameter request. + */ +class CameraSet: public StaticLocalDataSet { +public: + + // Size of dataset + static const size_t SIZE = 25; + + CameraSet(HasLocalDataPoolIF* owner) : + StaticLocalDataSet(owner, CAMERA_SET_ID) { + } + + CameraSet(object_id_t objectId) : + StaticLocalDataSet(sid_t(objectId, CAMERA_SET_ID)) { + } + + lp_var_t mode = lp_var_t(sid.objectId, PoolIds::CAMERA_MODE, this); + lp_var_t focallength = lp_var_t(sid.objectId, PoolIds::FOCALLENGTH, this); + lp_var_t exposure = lp_var_t(sid.objectId, PoolIds::EXPOSURE, this); + lp_var_t interval = lp_var_t(sid.objectId, PoolIds::INTERVAL, this); + lp_var_t offset = lp_var_t(sid.objectId, PoolIds::CAMERA_OFFSET, this); + lp_var_t pgagain = lp_var_t(sid.objectId, PoolIds::PGAGAIN, this); + lp_var_t adcgain = lp_var_t(sid.objectId, PoolIds::ADCGAIN, this); + lp_var_t reg1 = lp_var_t(sid.objectId, PoolIds::CAM_REG1, this); + lp_var_t val1 = lp_var_t(sid.objectId, PoolIds::CAM_VAL1, this); + lp_var_t reg2 = lp_var_t(sid.objectId, PoolIds::CAM_REG2, this); + lp_var_t val2 = lp_var_t(sid.objectId, PoolIds::CAM_VAL2, this); + lp_var_t reg3 = lp_var_t(sid.objectId, PoolIds::CAM_REG3, this); + lp_var_t val3 = lp_var_t(sid.objectId, PoolIds::CAM_VAL3, this); + lp_var_t reg4 = lp_var_t(sid.objectId, PoolIds::CAM_REG4, this); + lp_var_t val4 = lp_var_t(sid.objectId, PoolIds::CAM_VAL4, this); + + void printSet() { + PoolReadGuard rg(this); + sif::info << "CameraSet::printSet: mode: " << static_cast(this->mode.value) + << std::endl; + sif::info << "CameraSet::printSet: focallength: " << this->focallength << std::endl; + sif::info << "CameraSet::printSet: exposure: " << this->exposure << std::endl; + sif::info << "CameraSet::printSet: interval: " << this->interval << std::endl; + sif::info << "CameraSet::printSet: offset: " << this->offset << std::endl; + sif::info << "CameraSet::printSet: PGA gain: " + << static_cast(this->pgagain.value) << std::endl; + sif::info << "CameraSet::printSet: ADC gain: " + << static_cast(this->adcgain.value) << std::endl; + sif::info << "CameraSet::printSet: Reg 1: " + << static_cast(this->reg1.value) << std::endl; + sif::info << "CameraSet::printSet: Val 1: " + << static_cast(this->val1.value) << std::endl; + sif::info << "CameraSet::printSet: Reg 2: " + << static_cast(this->reg2.value) << std::endl; + sif::info << "CameraSet::printSet: Val 2: " + << static_cast(this->val2.value) << std::endl; + sif::info << "CameraSet::printSet: Reg 3: " + << static_cast(this->reg3.value) << std::endl; + sif::info << "CameraSet::printSet: Val 3: " + << static_cast(this->val3.value) << std::endl; + sif::info << "CameraSet::printSet: Reg 4: " + << static_cast(this->reg4.value) << std::endl; + sif::info << "CameraSet::printSet: Val 4: " + << static_cast(this->val4.value) << std::endl; + } +}; } #endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */ diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp index de70037a..efb54ff8 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp @@ -8,6 +8,7 @@ extern "C" { #include #include + #include #include "common/misc.h" } @@ -18,7 +19,7 @@ StarTrackerHandler::StarTrackerHandler(object_id_t objectId, object_id_t comIF, DeviceHandlerBase(objectId, comIF, comCookie), temperatureSet(this), versionSet(this), powerSet( this), interfaceSet(this), timeSet(this), solutionSet(this), histogramSet(this), contrastSet( this), checksumSet(this), downloadCentroidSet(this), downloadMatchedStar(this), - downloadDbImage(this), downloadBlobPixel(this), strHelper( + downloadDbImage(this), downloadBlobPixel(this), cameraSet(this), strHelper( strHelper) { if (comCookie == nullptr) { sif::error << "StarTrackerHandler: Invalid com cookie" << std::endl; @@ -495,6 +496,10 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi result = prepareFpgaActionCommand(commandData, commandDataLen); return result; } + case (StarTracker::REQ_CAMERA_PARAMS): { + result = prepareRequestCameraParams(); + return result; + } default: return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } @@ -573,6 +578,8 @@ void StarTrackerHandler::fillCommandAndReplyMap() { StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::FPGA_ACTION, 3, nullptr, StarTracker::MAX_FRAME_SIZE * 2 + 2); + this->insertInCommandAndReplyMap(StarTracker::REQ_CAMERA_PARAMS, 3, nullptr, + StarTracker::MAX_FRAME_SIZE * 2 + 2); } ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t remainingSize, @@ -604,7 +611,12 @@ ReturnValue_t StarTrackerHandler::scanForReply(const uint8_t *start, size_t rema } case TMTC_SETPARAMREPLY: { *foundLen = remainingSize - bytesLeft; - result = scanForParameterReply(foundId); + result = scanForSetParameterReply(foundId); + break; + } + case TMTC_PARAMREPLY: { + *foundLen = remainingSize - bytesLeft; + result = scanForGetParameterReply(foundId); break; } case TMTC_TELEMETRYREPLYA: @@ -726,6 +738,10 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con result = handleSetParamReply(); break; } + case (StarTracker::REQ_CAMERA_PARAMS): { + handleParamRequest(cameraSet, StarTracker::CameraSet::SIZE); + break; + } default: { sif::debug << "StarTrackerHandler::interpretDeviceReply: Unknown device reply id:" << id << std::endl; @@ -934,6 +950,22 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l localDataPoolMap.emplace(StarTracker::BLOBPIX_IN_USE, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::BLOBPIX_BRIGHT_NEIGHBOURS, new PoolEntry( { 0 })); localDataPoolMap.emplace(StarTracker::BLOBPIX_REGION, new PoolEntry( { 0 })); + + localDataPoolMap.emplace(StarTracker::CAMERA_MODE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::FOCALLENGTH, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::EXPOSURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::INTERVAL, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAMERA_OFFSET, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::PGAGAIN, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::ADCGAIN, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAM_REG1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAM_VAL1, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAM_REG2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAM_VAL2, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAM_REG3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAM_VAL3, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAM_REG4, new PoolEntry( { 0 })); + localDataPoolMap.emplace(StarTracker::CAM_VAL4, new PoolEntry( { 0 })); return RETURN_OK; } @@ -1036,7 +1068,7 @@ ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) return RETURN_OK; } -ReturnValue_t StarTrackerHandler::scanForParameterReply(DeviceCommandId_t *foundId) { +ReturnValue_t StarTrackerHandler::scanForSetParameterReply(DeviceCommandId_t *foundId) { const uint8_t* reply = dataLinkLayer.getReply(); switch (*reply) { case (StarTracker::ID::SUBSCRIBE): { @@ -1091,6 +1123,22 @@ ReturnValue_t StarTrackerHandler::scanForParameterReply(DeviceCommandId_t *found return RETURN_OK; } +ReturnValue_t StarTrackerHandler::scanForGetParameterReply(DeviceCommandId_t *foundId) { + const uint8_t* reply = dataLinkLayer.getReply(); + switch (*reply) { + case (StarTracker::ID::CAMERA): { + *foundId = StarTracker::REQ_CAMERA_PARAMS; + break; + } + default: { + sif::warning << "tarTrackerHandler::scanForGetParameterReply: UnkNown ID" << std::endl; + return RETURN_FAILED; + break; + } + } + return RETURN_OK; +} + ReturnValue_t StarTrackerHandler::scanForTmReply(DeviceCommandId_t *foundId) { const uint8_t* reply = dataLinkLayer.getReply(); switch (*reply) { @@ -1700,6 +1748,16 @@ ReturnValue_t StarTrackerHandler::prepareFpgaActionCommand(const uint8_t* comman return RETURN_OK; } +ReturnValue_t StarTrackerHandler::prepareRequestCameraParams() { + struct Camera req; + uint32_t length = 0; + arc_pack_camera_parameter_req(commandBuffer, &length); + dataLinkLayer.encodeFrame(commandBuffer, length); + rawPacket = dataLinkLayer.getEncodedFrame(); + rawPacketLen = dataLinkLayer.getEncodedLength(); + return RETURN_OK; +} + ReturnValue_t StarTrackerHandler::handleSetParamReply() { const uint8_t* reply = dataLinkLayer.getReply(); uint8_t status = *(reply + STATUS_OFFSET); @@ -1788,6 +1846,31 @@ ReturnValue_t StarTrackerHandler::handleChecksumReply() { return RETURN_OK; } +ReturnValue_t StarTrackerHandler::handleParamRequest(LocalPoolDataSetBase& dataset, size_t size) { + ReturnValue_t result = RETURN_OK; + result = dataset.read(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } + const uint8_t* reply = dataLinkLayer.getReply() + PARAMS_OFFSET; + dataset.setValidityBufferGeneration(false); + result = dataset.deSerialize(&reply, &size, SerializeIF::Endianness::LITTLE); + if (result != RETURN_OK) { + sif::warning << "StarTrackerHandler::handleParamRequest Deserialization failed" + << std::endl; + } + dataset.setValidityBufferGeneration(true); + dataset.setValidity(true, true); + result = dataset.commit(TIMEOUT_TYPE, MUTEX_TIMEOUT); + if (result != RETURN_OK) { + return result; + } +#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 + dataset.printSet(); +#endif + return result; +} + ReturnValue_t StarTrackerHandler::handlePingReply() { ReturnValue_t result = RETURN_OK; uint32_t pingId = 0; diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.h b/bsp_q7s/devices/startracker/StarTrackerHandler.h index 65bd65b8..0461af27 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.h +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.h @@ -142,6 +142,7 @@ private: // Max valid position value in upload image command static const uint16_t MAX_POSITION= 4095; static const uint8_t STATUS_OFFSET = 1; + static const uint8_t PARAMS_OFFSET = 1; static const uint8_t TICKS_OFFSET = 2; static const uint8_t TIME_OFFSET = 6; static const uint8_t TM_DATA_FIELD_OFFSET = 14; @@ -260,6 +261,7 @@ private: StarTracker::DownloadMatchedStar downloadMatchedStar; StarTracker::DownloadDBImage downloadDbImage; StarTracker::DownloadBlobPixel downloadBlobPixel; + StarTracker::CameraSet cameraSet; // Pointer to object responsible for uploading and downloading images to/from the star tracker StrHelper* strHelper = nullptr; @@ -318,7 +320,8 @@ private: void slipInit(); ReturnValue_t scanForActionReply(DeviceCommandId_t *foundId); - ReturnValue_t scanForParameterReply(DeviceCommandId_t *foundId); + ReturnValue_t scanForSetParameterReply(DeviceCommandId_t *foundId); + ReturnValue_t scanForGetParameterReply(DeviceCommandId_t *foundId); ReturnValue_t scanForTmReply(DeviceCommandId_t *foundId); /** @@ -492,6 +495,11 @@ private: */ ReturnValue_t prepareFpgaActionCommand(const uint8_t* commandData, size_t commandDataLen); + /** + * @brief Will fill the command buffer with the command to request the set camera parameters. + */ + ReturnValue_t prepareRequestCameraParams(); + /** * @brief Handles action replies with datasets. */ @@ -524,6 +532,8 @@ private: ReturnValue_t handlePingReply(); + ReturnValue_t handleParamRequest(LocalPoolDataSetBase& dataset, size_t size); + /** * @brief Checks the loaded program by means of the version set */