command to request camera parameters

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<CAMERA_SET_ENTRIES> {
+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<uint8_t> mode = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAMERA_MODE, this);
+    lp_var_t<float> focallength = lp_var_t<float>(sid.objectId, PoolIds::FOCALLENGTH, this);
+    lp_var_t<float> exposure = lp_var_t<float>(sid.objectId, PoolIds::EXPOSURE, this);
+    lp_var_t<float> interval = lp_var_t<float>(sid.objectId, PoolIds::INTERVAL, this);
+    lp_var_t<int16_t> offset = lp_var_t<int16_t>(sid.objectId, PoolIds::CAMERA_OFFSET, this);
+    lp_var_t<uint8_t> pgagain = lp_var_t<uint8_t>(sid.objectId, PoolIds::PGAGAIN, this);
+    lp_var_t<uint8_t> adcgain = lp_var_t<uint8_t>(sid.objectId, PoolIds::ADCGAIN, this);
+    lp_var_t<uint8_t> reg1 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_REG1, this);
+    lp_var_t<uint8_t> val1 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_VAL1, this);
+    lp_var_t<uint8_t> reg2 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_REG2, this);
+    lp_var_t<uint8_t> val2 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_VAL2, this);
+    lp_var_t<uint8_t> reg3 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_REG3, this);
+    lp_var_t<uint8_t> val3 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_VAL3, this);
+    lp_var_t<uint8_t> reg4 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_REG4, this);
+    lp_var_t<uint8_t> val4 = lp_var_t<uint8_t>(sid.objectId, PoolIds::CAM_VAL4, this);
+
+    void printSet() {
+        PoolReadGuard rg(this);
+        sif::info << "CameraSet::printSet: mode: " << static_cast<unsigned int>(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<unsigned int>(this->pgagain.value) << std::endl;
+        sif::info << "CameraSet::printSet: ADC gain: "
+                << static_cast<unsigned int>(this->adcgain.value) << std::endl;
+        sif::info << "CameraSet::printSet: Reg 1: "
+                << static_cast<unsigned int>(this->reg1.value) << std::endl;
+        sif::info << "CameraSet::printSet: Val 1: "
+                << static_cast<unsigned int>(this->val1.value) << std::endl;
+        sif::info << "CameraSet::printSet: Reg 2: "
+                << static_cast<unsigned int>(this->reg2.value) << std::endl;
+        sif::info << "CameraSet::printSet: Val 2: "
+                << static_cast<unsigned int>(this->val2.value) << std::endl;
+        sif::info << "CameraSet::printSet: Reg 3: "
+                << static_cast<unsigned int>(this->reg3.value) << std::endl;
+        sif::info << "CameraSet::printSet: Val 3: "
+                << static_cast<unsigned int>(this->val3.value) << std::endl;
+        sif::info << "CameraSet::printSet: Reg 4: "
+                << static_cast<unsigned int>(this->reg4.value) << std::endl;
+        sif::info << "CameraSet::printSet: Val 4: "
+                << static_cast<unsigned int>(this->val4.value) << std::endl;
+    }
+};
 }
 #endif /* MISSION_STARTRACKER_DEFINITIONS_H_ */

bsp_q7s/devices/startracker/StarTrackerHandler.cpp

@@ -8,6 +8,7 @@
 extern "C" {
     #include <thirdparty/arcsec_star_tracker/client/generated/telemetry.h>
     #include <thirdparty/arcsec_star_tracker/client/generated/actionreq.h>
+    #include <thirdparty/arcsec_star_tracker/client/generated/parameter.h>
     #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<uint8_t>( { 0 }));
     localDataPoolMap.emplace(StarTracker::BLOBPIX_BRIGHT_NEIGHBOURS, new PoolEntry<uint32_t>( { 0 }));
     localDataPoolMap.emplace(StarTracker::BLOBPIX_REGION, new PoolEntry<uint32_t>( { 0 }));
+
+    localDataPoolMap.emplace(StarTracker::CAMERA_MODE, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::FOCALLENGTH, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::EXPOSURE, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::INTERVAL, new PoolEntry<float>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAMERA_OFFSET, new PoolEntry<int16_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::PGAGAIN, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::ADCGAIN, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAM_REG1, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAM_VAL1, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAM_REG2, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAM_VAL2, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAM_REG3, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAM_VAL3, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAM_REG4, new PoolEntry<uint8_t>( { 0 }));
+    localDataPoolMap.emplace(StarTracker::CAM_VAL4, new PoolEntry<uint8_t>( { 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;

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
 	 */