Committing Atomos-used algorithms that have problematic effector mana…

…gement for #817

src/fswAlgorithms/effectorInterfaces/rwMotorTorque/rwMotorTorque.c

@@ -46,6 +46,7 @@ void SelfInit_rwMotorTorque(rwMotorTorqueConfig *configData, int64_t moduleID)
  */
 void Reset_rwMotorTorque(rwMotorTorqueConfig *configData, uint64_t callTime, int64_t moduleID)
 {
+    ArrayMotorTorqueMsgPayload rwMotorTorques;      /*!< Msg struct to store the output message */
     double *pAxis;                 /* pointer to the current control axis */
     int i;
 
@@ -79,6 +80,10 @@ void Reset_rwMotorTorque(rwMotorTorqueConfig *configData, uint64_t callTime, int
             v3Copy(&configData->rwConfigParams.GsMatrix_B[i * 3], &configData->GsMatrix_B[i * 3]);
         }
     }
+
+    /* zero the RW motor torque output message */
+    rwMotorTorques = ArrayMotorTorqueMsg_C_zeroMsgPayload();
+    ArrayMotorTorqueMsg_C_write(&rwMotorTorques, &configData->rwMotorTorqueOutMsg, moduleID, callTime);
 }
 
 /*! Add a description of what this main Update() routine does for this module

src/fswAlgorithms/effectorInterfaces/rwNullSpace/rwNullSpace.c

@@ -52,6 +52,7 @@ void Reset_rwNullSpace(rwNullSpaceConfig *configData, uint64_t callTime,
     double identMatrix[MAX_EFF_CNT*MAX_EFF_CNT];    /* [-]  [I_NxN] identity matrix */
     double GsTemp[MAX_EFF_CNT*MAX_EFF_CNT];         /* [-]  temp matrix */
     RWConstellationMsgPayload localRWData;          /*      local copy of RW configuration data */
+    ArrayMotorTorqueMsgPayload finalControl;
 
     // check if the required input messages are included
     if (!RWConstellationMsg_C_isLinked(&configData->rwConfigInMsg)) {
@@ -93,6 +94,8 @@ void Reset_rwNullSpace(rwNullSpaceConfig *configData, uint64_t callTime,
     mSubtract(identMatrix, configData->numWheels, configData->numWheels,    /* find ([I] - [Gs]^T.([Gs].[Gs]^T)^-1.[Gs]) */
               GsTemp, configData->tau);
 
+    finalControl = ArrayMotorTorqueMsg_C_zeroMsgPayload();
+    ArrayMotorTorqueMsg_C_write(&finalControl, &configData->rwMotorTorqueOutMsg, moduleID, callTime);
 }
 
 /*! This method takes the input reaction wheel commands as well as the observed

src/fswAlgorithms/effectorInterfaces/thrFiringSchmitt/thrFiringSchmitt.c

@@ -54,6 +54,7 @@ void SelfInit_thrFiringSchmitt(thrFiringSchmittConfig *configData, int64_t modul
  */
 void Reset_thrFiringSchmitt(thrFiringSchmittConfig *configData, uint64_t callTime, int64_t moduleID)
 {
+	THRArrayOnTimeCmdMsgPayload thrOnTimeOut;       /* -- copy of the thruster on-time output message */
 	THRArrayConfigMsgPayload   localThrusterData;     /* local copy of the thruster data message */
 	int 				i;
 
@@ -78,6 +79,10 @@ void Reset_thrFiringSchmitt(thrFiringSchmittConfig *configData, uint64_t callTim
 		configData->maxThrust[i] = localThrusterData.thrusters[i].maxThrust;
 		configData->lastThrustState[i] = BOOL_FALSE;
 	}
+
+	/* zero the thruster on-time command output message */
+	thrOnTimeOut = THRArrayOnTimeCmdMsg_C_zeroMsgPayload();
+	THRArrayOnTimeCmdMsg_C_write(&thrOnTimeOut, &configData->onTimeOutMsg, moduleID, callTime);
 }
 
 /*! This method maps the input thruster command forces into thruster on times using a remainder tracking logic.

src/fswAlgorithms/effectorInterfaces/thrForceMapping/thrForceMapping.c

@@ -47,6 +47,7 @@ void SelfInit_thrForceMapping(thrForceMappingConfig *configData, int64_t moduleI
  */
 void Reset_thrForceMapping(thrForceMappingConfig *configData, uint64_t callTime, int64_t moduleID)
 {
+    THRArrayCmdForceMsgPayload thrusterForceOut;
     double             *pAxis;                  /* pointer to the current control axis */
     uint32_t                 i;
     THRArrayConfigMsgPayload   localThrusterData;   /* local copy of the thruster data message */
@@ -97,6 +98,10 @@ void Reset_thrForceMapping(thrForceMappingConfig *configData, uint64_t callTime,
             configData->thrForcMag[i] = localThrusterData.thrusters[i].maxThrust;
         }
     }
+
+    /* zero the RCS thrust force command output message */
+    thrusterForceOut = THRArrayCmdForceMsg_C_zeroMsgPayload();
+    THRArrayCmdForceMsg_C_write(&thrusterForceOut, &configData->thrForceCmdOutMsg, moduleID, callTime);
 }
 
 /*! The module takes a body frame torque vector and projects it onto available RCS or DV thrusters.

src/simulation/dynamics/reactionWheels/reactionWheelStateEffector.cpp

@@ -399,6 +399,7 @@ void ReactionWheelStateEffector::Reset(uint64_t CurrenSimNanos)
 
     /* zero the RW wheel output message buffer */
     this->rwSpeedMsgBuffer = this->rwSpeedOutMsg.zeroMsgPayload;
+    this->rwSpeedOutMsg.write(&rwSpeedMsgBuffer, this->moduleID, CurrenSimNanos);
 }
 
 /*! This method is here to write the output message structure into the specified