.clang-format

@@ -1,6 +1,9 @@
 ---
-BasedOnStyle: Mozilla
+BasedOnStyle: Google
 ColumnLimit: '120'
 IndentWidth: '4'
 TabWidth:    '4'
+BinPackArguments: false
+BinPackParameters: false
+AllowAllParametersOfDeclarationOnNextLine: false
 ...

.pre-commit-config.yaml

@@ -1,13 +1,18 @@
 exclude: '.*\.(tls|bsp|tpc|bc)$'
 repos:
 -   repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v2.3.0
+    rev: v5.0.0
     hooks:
-      -   id: trailing-whitespace
-      -   id: end-of-file-fixer
-      -   id: check-yaml
       -   id: check-added-large-files
           args: ['--maxkb=10000']
       -   id: check-case-conflict
       -   id: check-merge-conflict
+      -   id: check-yaml
       -   id: debug-statements
+      -   id: end-of-file-fixer
+      -   id: trailing-whitespace
+-   repo: https://github.com/pocc/pre-commit-hooks
+    rev: v1.3.5
+    hooks:
+      -   id: clang-format
+          args: [-i, -style=file]

requirements.txt

@@ -1,6 +1,7 @@
 bokeh
 conan>=1.40.1, <2.00.0
 datashader==0.16.3
+delimited-protobuf==1.0.0
 holoviews==1.19.1
 hvplot==0.10.0
 opencv-contrib-python
@@ -9,6 +10,7 @@ pandas
 param==2.1.1
 parse>=1.18.0
 Pillow
+protobuf==3.19.1
 pytest
 pytest-html
 pytest-xdist

src/architecture/msgPayloadDefC/CameraRenderingMsgPayload.h

@@ -27,7 +27,7 @@
 typedef struct {
     int cameraId;
     double cosmicRayStdDeviation;
-    bool enableStrayLight;
+    double strayLight;
     bool starField;
     char rendering[MAX_STRING_LENGTH];
     bool smear;

src/architecture/msgPayloadDefC/CelestialBodyParametersMsgPayload.h

@@ -28,12 +28,13 @@
 typedef struct {
     char bodyName[MAX_STRING_LENGTH];
     char shapeModel[MAX_STRING_LENGTH];
+    double sigma_BN[3];
     double perlinNoise;
     double proceduralRocks;
     char brdf[MAX_STRING_LENGTH];
     double reflectanceParameters[MAX_PARAMETER_LENGTH];
     double meanRadius;
-    double principalAxisDistortion;
+    double principalAxisDistortion[3];
 }CelestialBodyParametersMsgPayload;
 
 #endif //CELESTIAL_BODY_PARAMETERS

src/architecture/msgPayloadDefCpp/CameraModelMsgPayload.h

@@ -34,10 +34,9 @@ typedef struct {
     double bodyToCameraMrp[3];        //!< [-] MRP defining the orientation of the camera frame relative to the body frame */
     double focalLength;
     int gaussianPointSpreadFunction;  //!< Size of square Gaussian kernel to model point spread function, must be odd
-    double cosmicRayFrequency;  //!< Frequency at which cosmic rays can strike the camera
+    double exposureTime;  //!< [s] Exposure time for each image taken
     double readNoise;  //!< Read noise standard deviation
     double systemGain;  //!< Mapping from current to pixel intensity
-    bool enableStrayLight;  //!< Add basic stray light modelling to images
 }CameraModelMsgPayload;
 
 #endif

src/fswAlgorithms/attGuidance/flybyPoint/_UnitTest/test_flybyPoint.py

@@ -43,8 +43,9 @@
 @pytest.mark.parametrize("orbit_normal_sign", [1, -1])
 @pytest.mark.parametrize("max_rate", [0, 0.01])
 @pytest.mark.parametrize("max_acceleration", [0, 1E-7])
+@pytest.mark.parametrize("pos_knowledge", [0, 1E5])
 def test_flybyPoint(show_plots, initial_position, initial_velocity, filter_dt, orbit_normal_sign, max_rate,
-                    max_acceleration):
+                    max_acceleration, pos_knowledge):
     r"""
     **Validation Test Description**
 
@@ -76,11 +77,11 @@ def test_flybyPoint(show_plots, initial_position, initial_velocity, filter_dt, o
     """
     # each test method requires a single assert method to be called
     flybyPointTestFunction(show_plots, initial_position, initial_velocity, filter_dt, orbit_normal_sign,
-                           max_rate, max_acceleration)
+                           max_rate, max_acceleration, pos_knowledge)
 
 
 def flybyPointTestFunction(show_plots, initial_position, initial_velocity, filter_dt, orbit_normal_sign,
-                           max_rate, max_acceleration):
+                           max_rate, max_acceleration, pos_knowledge):
     # setup simulation environment
     sim_dt = 10
     unit_test_sim = SimulationBaseClass.SimBaseClass()
@@ -96,6 +97,7 @@ def flybyPointTestFunction(show_plots, initial_position, initial_velocity, filte
     flyby_guidance.setSignOfOrbitNormalFrameVector(orbit_normal_sign)
     flyby_guidance.setMaximumRateThreshold(max_rate)
     flyby_guidance.setMaximumAccelerationThreshold(max_acceleration)
+    flyby_guidance.setPositionKnowledgeSigma(pos_knowledge)
     unit_test_sim.AddModelToTask("unit_task", flyby_guidance)
 
     input_data = messaging.NavTransMsgPayload()

src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.cpp

@@ -46,12 +46,15 @@ void FlybyPoint::updateState(uint64_t currentSimNanos)
         /*! If this is the first read, seed the algorithm with the solution  */
         auto [r_BN_N, v_BN_N] = this->readRelativeState();
         if (this->firstRead){
+            this->timeOfFirstRead = currentSimNanos*NANO2SEC;
+            this->firstNavPosition = r_BN_N;
+            this->firstNavVelocity = v_BN_N;
             this->computeFlybyParameters(r_BN_N, v_BN_N);
             this->computeRN(r_BN_N, v_BN_N);
             this->firstRead = false;
         }
         /*! Protect against bad new solutions by checking validity */
-        if (this->checkValidity(r_BN_N, v_BN_N)) {
+        else if (this->checkValidity(currentSimNanos, r_BN_N, v_BN_N)) {
             /*! update flyby parameters and guidance frame */
             this->computeFlybyParameters(r_BN_N, v_BN_N);
             this->computeRN(r_BN_N, v_BN_N);
@@ -89,7 +92,7 @@ void FlybyPoint::computeFlybyParameters(Eigen::Vector3d &r_BN_N, Eigen::Vector3d
     this->gamma0 = std::atan(v_BN_N.dot(ur_N) / v_BN_N.dot(ut_N));
 }
 
-bool FlybyPoint::checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const{
+bool FlybyPoint::checkValidity(uint64_t currentSimNanos, Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const{
     bool valid = true;
     Eigen::Vector3d ur_N = r_BN_N.normalized();
     Eigen::Vector3d uv_N = v_BN_N.normalized();
@@ -110,6 +113,11 @@ bool FlybyPoint::checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N)
     if (maxPredictedAcceleration > this->maxAcceleration && this->maxAcceleration > 0) {
         valid = false;
     }
+    double deltaT = currentSimNanos*NANO2SEC - this->timeOfFirstRead;
+    double deltaPositionNorm = (r_BN_N - (this->firstNavPosition + deltaT*this->firstNavVelocity)).norm();
+    if (deltaPositionNorm > this->positionKnowledgeSigma && this->positionKnowledgeSigma > 0) {
+        valid = false;
+    }
 
     return valid;
 }
@@ -232,3 +240,17 @@ double FlybyPoint::getMaximumRateThreshold() const {
 void FlybyPoint::setMaximumRateThreshold(double maxRateThreshold) {
     this->maxRate = maxRateThreshold;
 }
+
+/*! Get the ground based positional knowledge standard deviation
+ @return sigma
+ */
+double FlybyPoint::getPositionKnowledgeSigma() const {
+    return this->positionKnowledgeSigma;
+}
+
+/*! Set the ground based positional knowledge sigma
+ @param sigma
+ */
+void FlybyPoint::setPositionKnowledgeSigma(double positionKnowledgeStd) {
+    this->positionKnowledgeSigma = positionKnowledgeStd;
+}

src/fswAlgorithms/attGuidance/flybyPoint/flybyPoint.h

@@ -36,7 +36,7 @@ class FlybyPoint: public SysModel {
     void updateState(uint64_t currentSimNanos) override;
 
     std::tuple<Eigen::Vector3d, Eigen::Vector3d> readRelativeState();
-    bool checkValidity(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const;
+    bool checkValidity(uint64_t currentSimNanos, Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N) const;
     void computeFlybyParameters(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N);
     void computeRN(Eigen::Vector3d &r_BN_N, Eigen::Vector3d &v_BN_N);
     std::tuple<Eigen::Vector3d, Eigen::Vector3d, Eigen::Vector3d> computeGuidanceSolution() const;
@@ -55,13 +55,16 @@ class FlybyPoint: public SysModel {
     void setMaximumAccelerationThreshold(double maxAccelerationThreshold);
     double getMaximumRateThreshold() const;
     void setMaximumRateThreshold(double maxRateThreshold);
+    double getPositionKnowledgeSigma() const;
+    void setPositionKnowledgeSigma(double positionKnowledgeStd);
 
-    ReadFunctor<NavTransMsgPayload>  filterInMsg;               //!< input msg relative position w.r.t. asteroid
+    ReadFunctor<NavTransMsgPayload> filterInMsg;               //!< input msg relative position w.r.t. asteroid
     ReadFunctor<EphemerisMsgPayload> asteroidEphemerisInMsg;    //!< input asteroid ephemeris msg
     Message<AttRefMsgPayload> attRefOutMsg;                     //!< Attitude reference output message
 
 private:
     double dt = 0; //!< current time step between last two updates
+    double timeOfFirstRead = 0;  //!< time of first nav solution read
     double timeBetweenFilterData = 0;       //!< time between two subsequent reads of the filter information
     double toleranceForCollinearity = 0;            //!< tolerance for singular conditions when position and velocity are collinear
     int signOfOrbitNormalFrameVector = 1;  //!< Sign of orbit normal vector to complete reference frame
@@ -74,6 +77,9 @@ class FlybyPoint: public SysModel {
     double gamma0 = 0;              //!< flight path angle of the spacecraft at time of read [rad]
     uint64_t lastFilterReadTime = 0;  //!< time of last filter read
     Eigen::Matrix3d R0N;           //!< inertial-to-reference DCM at time of read
+    Eigen::Vector3d firstNavPosition{};           //!< First position used to create profile
+    Eigen::Vector3d firstNavVelocity{};           //!< First velocity used to create profile
+    double positionKnowledgeSigma = 0;           //!< Last position used to create profile
 
 };
 

src/fswAlgorithms/imageProcessing/centerOfBrightness/centerOfBrightness.cpp

@@ -110,8 +110,8 @@ void CenterOfBrightness::updateState(uint64_t currentSimNanos)
             cobBuffer.valid = true;
             cobBuffer.timeTag = this->sensorTimeTag;
             cobBuffer.cameraID = imageBuffer.cameraID;
-            cobBuffer.centerOfBrightness[0] = cobData.first[0];
-            cobBuffer.centerOfBrightness[1] = cobData.first[1];
+            cobBuffer.centerOfBrightness[0] = cobData.first[0] + 0.5;
+            cobBuffer.centerOfBrightness[1] = cobData.first[1] + 0.5;
             cobBuffer.pixelsFound = static_cast<int32_t> (locations.size());
         }
         cobBuffer.rollingAverageBrightness = averageBrightnessNew;

src/fswAlgorithms/opticalNavigation/cobConverter/_UnitTest/test_cobConverter.py

@@ -69,8 +69,8 @@ def compute_camera_calibration_matrix(input_camera):
     alpha = 0.
     resX = input_camera.resolution[0]
     resY = input_camera.resolution[1]
-    pX = 2. * np.tan(input_camera.fieldOfView / 2.0)
-    pY = 2. * np.tan(input_camera.fieldOfView * resY / resX / 2.0)
+    pX = 2. * np.tan(input_camera.fieldOfView[0] / 2.0)
+    pY = 2. * np.tan(input_camera.fieldOfView[0] * resY / resX / 2.0)
     dX = resX / pX
     dY = resY / pY
     up = resX / 2.
@@ -145,18 +145,18 @@ def cob_converter_test_function(show_plots, cameraResolution, centerOfBrightness
     sigma_CB = rbk.C2MRP(dcm_CB)
 
     # Create the input messages.
-    inputCamera = messaging.CameraConfigMsgPayload()
+    inputCamera = messaging.CameraModelMsgPayload()
     inputCob = messaging.OpNavCOBMsgPayload()
     inputFilter = messaging.FilterMsgPayload()
     inputAtt = messaging.NavAttMsgPayload()
     inputEphem = messaging.EphemerisMsgPayload()
 
     # Set camera parameters
-    inputCamera.fieldOfView = np.deg2rad(20.0)
+    inputCamera.fieldOfView = [np.deg2rad(20.0), np.deg2rad(20.0)]
     inputCamera.resolution = cameraResolution
-    inputCamera.sigma_CB = sigma_CB
-    inputCamera.ppFocalLength = 0.10
-    camInMsg = messaging.CameraConfigMsg().write(inputCamera)
+    inputCamera.bodyToCameraMrp = sigma_CB
+    inputCamera.focalLength = 0.10
+    camInMsg = messaging.CameraModelMsg().write(inputCamera)
     module.cameraConfigInMsg.subscribeTo(camInMsg)
 
     # Set center of brightness
@@ -211,7 +211,7 @@ def cob_converter_test_function(show_plots, cameraResolution, centerOfBrightness
         goodPixels = 0
     cob_true = [inputCob.centerOfBrightness[0], inputCob.centerOfBrightness[1]]
     num_pixels = inputCob.pixelsFound
-    dcm_CB = rbk.MRP2C(inputCamera.sigma_CB)
+    dcm_CB = rbk.MRP2C(inputCamera.bodyToCameraMrp)
     dcm_BN = rbk.MRP2C(inputAtt.sigma_BN)
     dcm_NC = np.dot(dcm_CB, dcm_BN).T
 
@@ -238,8 +238,8 @@ def cob_converter_test_function(show_plots, cameraResolution, centerOfBrightness
     phi = np.arctan2(shat_C[1], shat_C[0])  # sun direction in image plane
     K = compute_camera_calibration_matrix(inputCamera)
     dX = K[0, 0]
-    Kx = dX / inputCamera.ppFocalLength
-    Rc = R_object * Kx * inputCamera.ppFocalLength / np.linalg.norm(r_BdyZero_N)  # object radius in pixels
+    Kx = dX / inputCamera.focalLength
+    Rc = R_object * Kx * inputCamera.focalLength / np.linalg.norm(r_BdyZero_N)  # object radius in pixels
     com_true = [None] * 2  # COM location in image
     com_true[0] = cob_true[0] - gamma * Rc * np.cos(phi) * goodPixels
     com_true[1] = cob_true[1] - gamma * Rc * np.sin(phi) * goodPixels

src/fswAlgorithms/opticalNavigation/cobConverter/cobConverter.cpp

@@ -67,7 +67,7 @@ void CobConverter::reset(uint64_t currentSimNanos)
  */
 void CobConverter::updateState(uint64_t currentSimNanos)
 {
-    CameraConfigMsgPayload cameraSpecs = this->cameraConfigInMsg();
+    CameraModelMsgPayload cameraSpecs = this->cameraConfigInMsg();
     OpNavCOBMsgPayload cobMsgBuffer = this->opnavCOBInMsg();
     NavAttMsgPayload navAttBuffer = this->navAttInMsg();
     EphemerisMsgPayload ephemBuffer = this->ephemInMsg();
@@ -83,7 +83,7 @@ void CobConverter::updateState(uint64_t currentSimNanos)
         /*! - Extract rotations from relevant messages */
         double CB[3][3];
         double BN[3][3];
-        MRP2C(cameraSpecs.sigma_CB, CB);
+        MRP2C(cameraSpecs.bodyToCameraMrp, CB);
         Eigen::Matrix3d dcm_CB = c2DArray2EigenMatrix3d(CB);
         MRP2C(navAttBuffer.sigma_BN, BN);
         Eigen::Matrix3d dcm_BN = c2DArray2EigenMatrix3d(BN);
@@ -92,7 +92,7 @@ void CobConverter::updateState(uint64_t currentSimNanos)
 
         /*! - camera parameters */
         double alpha = 0;
-        double fieldOfView = cameraSpecs.fieldOfView;
+        double fieldOfView = cameraSpecs.fieldOfView[0];
         double resolutionX = cameraSpecs.resolution[0];
         double resolutionY = cameraSpecs.resolution[1];
         double pX = 2.*tan(fieldOfView/2.0);
@@ -234,7 +234,7 @@ void CobConverter::updateState(uint64_t currentSimNanos)
         comMsgBuffer.objectPixelRadius = int(Rc);
         comMsgBuffer.phaseAngle = alphaPA;
         comMsgBuffer.sunDirection = phi;
-        comMsgBuffer.cameraID = cameraSpecs.cameraID;
+        comMsgBuffer.cameraID = cameraSpecs.cameraId;
         comMsgBuffer.timeTag = cobMsgBuffer.timeTag;
         comMsgBuffer.valid = validCOM;
     }

src/fswAlgorithms/opticalNavigation/cobConverter/cobConverter.h

@@ -25,7 +25,7 @@
 #include "architecture/utilities/avsEigenSupport.h"
 #include "architecture/messaging/messaging.h"
 
-#include "architecture/msgPayloadDefC/CameraConfigMsgPayload.h"
+#include "architecture/msgPayloadDefCpp/CameraModelMsgPayload.h"
 #include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
 #include "architecture/msgPayloadDefC/EphemerisMsgPayload.h"
 #include "architecture/msgPayloadDefCpp/OpNavCOBMsgPayload.h"
@@ -38,6 +38,7 @@
 #include "architecture/utilities/linearAlgebra.h"
 #include "architecture/utilities/avsEigenMRP.h"
 #include "architecture/utilities/bskLogging.h"
+#include "architecture/utilities/macroDefinitions.h"
 
 enum class PhaseAngleCorrectionMethod {NoCorrection, Lambertian, Binary};
 
@@ -78,7 +79,7 @@ class CobConverter: public SysModel {
     Message<OpNavCOMMsgPayload> opnavCOMOutMsg;
     ReadFunctor<OpNavCOBMsgPayload> opnavCOBInMsg;
     ReadFunctor<FilterMsgPayload> opnavFilterInMsg;
-    ReadFunctor<CameraConfigMsgPayload> cameraConfigInMsg;
+    ReadFunctor<CameraModelMsgPayload> cameraConfigInMsg;
     ReadFunctor<NavAttMsgPayload> navAttInMsg;
     ReadFunctor<EphemerisMsgPayload> ephemInMsg;
 

src/fswAlgorithms/opticalNavigation/cobConverter/cobConverter.i

@@ -33,12 +33,9 @@ from Basilisk.architecture.swig_common_model import *
 
 %include "cobConverter.h"
 
-%include "architecture/msgPayloadDefC/CameraConfigMsgPayload.h"
-struct CameraConfigMsg_C;
+%include "architecture/msgPayloadDefCpp/CameraModelMsgPayload.h"
 %include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
-struct NavAttMsg_C;
 %include "architecture/msgPayloadDefC/EphemerisMsgPayload.h"
-struct EphemerisMsg_C;
 
 %include "architecture/msgPayloadDefCpp/OpNavUnitVecMsgPayload.h"
 %include "architecture/msgPayloadDefCpp/OpNavCOBMsgPayload.h"