Source code for BSK_MultiSatFsw

#
#  ISC License
#
#  Copyright (c) 2021, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
#
#  Permission to use, copy, modify, and/or distribute this software for any
#  purpose with or without fee is hereby granted, provided that the above
#  copyright notice and this permission notice appear in all copies.
#
#  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
#  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
#  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
#  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
#  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
#  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
#  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
#

import itertools

import numpy as np
from Basilisk.architecture import messaging
from Basilisk.fswAlgorithms import (inertial3D, locationPointing, attTrackingError, mrpFeedback,
                                    rwMotorTorque, spacecraftReconfig)
from Basilisk.utilities import (macros as mc, fswSetupThrusters)
from Basilisk.utilities import deprecated


[docs] class BSKFswModels: """Defines the FSW class""" def __init__(self, SimBase, fswRate, spacecraftIndex): # define empty class variables self.spacecraftIndex = spacecraftIndex self.decayTime = None self.xi = None self.modeRequest = "standby" self.stationKeeping = "OFF" self.vcMsg = None self.fswRwConfigMsg = None self.fswThrusterConfigMsg = None self.cmdTorqueMsg = None self.cmdTorqueDirectMsg = None self.attRefMsg = None self.attGuidMsg = None self.cmdRwMotorMsg = None # Define process name and default time-step for all FSW tasks defined later on self.processName = SimBase.FSWProcessName[spacecraftIndex] self.processTasksTimeStep = mc.sec2nano(fswRate) # Create tasks SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("inertialPointTask" + str(spacecraftIndex), self.processTasksTimeStep), 20) SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("sunPointTask" + str(spacecraftIndex), self.processTasksTimeStep), 20) SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("locPointTask" + str(spacecraftIndex), self.processTasksTimeStep), 20) SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("spacecraftReconfigTask" + str(spacecraftIndex), self.processTasksTimeStep), 15) SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("trackingErrorTask" + str(spacecraftIndex), self.processTasksTimeStep), 10) SimBase.fswProc[spacecraftIndex].addTask(SimBase.CreateNewTask("mrpFeedbackRWsTask" + str(spacecraftIndex), self.processTasksTimeStep), 5) # Create module data and module wraps self.inertial3DPoint = inertial3D.inertial3D() self.inertial3DPoint.ModelTag = "inertial3D" self.sunPoint = locationPointing.locationPointing() self.sunPoint.ModelTag = "sunPoint" self.locPoint = locationPointing.locationPointing() self.locPoint.ModelTag = "locPoint" self.spacecraftReconfig = spacecraftReconfig.spacecraftReconfig() self.spacecraftReconfig.ModelTag = "spacecraftReconfig" self.trackingError = attTrackingError.attTrackingError() self.trackingError.ModelTag = "trackingError" self.mrpFeedbackRWs = mrpFeedback.mrpFeedback() self.mrpFeedbackRWs.ModelTag = "mrpFeedbackRWs" self.rwMotorTorque = rwMotorTorque.rwMotorTorque() self.rwMotorTorque.ModelTag = "rwMotorTorque" # create the FSW module gateway messages self.setupGatewayMsgs(SimBase) # Initialize all modules self.InitAllFSWObjects(SimBase) # Assign initialized modules to tasks SimBase.AddModelToTask("inertialPointTask" + str(spacecraftIndex), self.inertial3DPoint, 10) SimBase.AddModelToTask("sunPointTask" + str(spacecraftIndex), self.sunPoint, 10) SimBase.AddModelToTask("locPointTask" + str(spacecraftIndex), self.locPoint, 10) SimBase.AddModelToTask("spacecraftReconfigTask" + str(spacecraftIndex), self.spacecraftReconfig, 10) SimBase.AddModelToTask("trackingErrorTask" + str(spacecraftIndex), self.trackingError, 9) SimBase.AddModelToTask("mrpFeedbackRWsTask" + str(spacecraftIndex), self.mrpFeedbackRWs, 7) SimBase.AddModelToTask("mrpFeedbackRWsTask" + str(spacecraftIndex), self.rwMotorTorque, 6) # Create events to be called for triggering GN&C maneuvers SimBase.fswProc[spacecraftIndex].disableAllTasks() # The standby event should not be active while the station keeping mode is also active. Standby mode disables # attitude control and therefore the attitude cannot be corrected for orbital correction burns. SimBase.createNewEvent("initiateStandby_" + str(spacecraftIndex), self.processTasksTimeStep, True, ["self.FSWModels[" + str(spacecraftIndex) + "].modeRequest == 'standby'"], ["self.fswProc[" + str(spacecraftIndex) + "].disableAllTasks()", "self.FSWModels[" + str(spacecraftIndex) + "].zeroGateWayMsgs()", "self.setAllButCurrentEventActivity('initiateStandby_" + str(spacecraftIndex) + "', True, useIndex=True)"]) SimBase.createNewEvent("initiateInertialPointing_" + str(spacecraftIndex), self.processTasksTimeStep, True, ["self.FSWModels[" + str(spacecraftIndex) + "].modeRequest == 'inertialPointing'"], ["self.fswProc[" + str(spacecraftIndex) + "].disableAllTasks()", "self.FSWModels[" + str(spacecraftIndex) + "].zeroGateWayMsgs()", "self.enableTask('inertialPointTask" + str(spacecraftIndex) + "')", "self.enableTask('trackingErrorTask" + str(spacecraftIndex) + "')", "self.enableTask('mrpFeedbackRWsTask" + str(spacecraftIndex) + "')", "self.setAllButCurrentEventActivity('initiateInertialPointing_" + str(spacecraftIndex) + "', True, useIndex=True)"]) SimBase.createNewEvent("initiateSunPointing_" + str(spacecraftIndex), self.processTasksTimeStep, True, ["self.FSWModels[" + str(spacecraftIndex) + "].modeRequest == 'sunPointing'"], ["self.fswProc[" + str(spacecraftIndex) + "].disableAllTasks()", "self.FSWModels[" + str(spacecraftIndex) + "].zeroGateWayMsgs()", "self.enableTask('sunPointTask" + str(spacecraftIndex) + "')", "self.enableTask('trackingErrorTask" + str(spacecraftIndex) + "')", "self.enableTask('mrpFeedbackRWsTask" + str(spacecraftIndex) + "')", "self.setAllButCurrentEventActivity('initiateSunPointing_" + str(spacecraftIndex) + "', True, useIndex=True)"]) SimBase.createNewEvent("initiateLocationPointing_" + str(spacecraftIndex), self.processTasksTimeStep, True, ["self.FSWModels[" + str(spacecraftIndex) + "].modeRequest == 'locationPointing'"], ["self.fswProc[" + str(spacecraftIndex) + "].disableAllTasks()", "self.FSWModels[" + str(spacecraftIndex) + "].zeroGateWayMsgs()", "self.enableTask('locPointTask" + str(spacecraftIndex) + "')", "self.enableTask('trackingErrorTask" + str(spacecraftIndex) + "')", "self.enableTask('mrpFeedbackRWsTask" + str(spacecraftIndex) + "')", "self.setAllButCurrentEventActivity('initiateLocationPointing_" + str(spacecraftIndex) + "', True, useIndex=True)"]) SimBase.createNewEvent("initiateStationKeeping_" + str(spacecraftIndex), self.processTasksTimeStep, True, ["self.FSWModels[" + str(spacecraftIndex) + "].stationKeeping == 'ON'"], ["self.enableTask('spacecraftReconfigTask" + str(spacecraftIndex) + "')", "self.setEventActivity('stopStationKeeping_" + str(spacecraftIndex) + "', True)"]) SimBase.createNewEvent("stopStationKeeping_" + str(spacecraftIndex), self.processTasksTimeStep, True, ["self.FSWModels[" + str(spacecraftIndex) + "].stationKeeping == 'OFF'"], ["self.disableTask('spacecraftReconfigTask" + str(spacecraftIndex) + "')", "self.setEventActivity('initiateStationKeeping_" + str(spacecraftIndex) + "', True)"]) # ------------------------------------------------------------------------------------------- # # These are module-initialization methods
[docs] def SetInertial3DPointGuidance(self): """ Defines the inertial pointing guidance module. """ self.inertial3DPoint.sigma_R0N = [0.1, 0.2, -0.3] messaging.AttRefMsg_C_addAuthor(self.inertial3DPoint.attRefOutMsg, self.attRefMsg)
[docs] def SetSunPointGuidance(self, SimBase): """ Defines the Sun pointing guidance module. """ self.sunPoint.pHat_B = SimBase.DynModels[self.spacecraftIndex].solarPanelAxis self.sunPoint.scAttInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg) self.sunPoint.scTransInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg) self.sunPoint.celBodyInMsg.subscribeTo(SimBase.EnvModel.ephemObject.ephemOutMsgs[SimBase.EnvModel.sun]) messaging.AttRefMsg_C_addAuthor(self.sunPoint.attRefOutMsg, self.attRefMsg)
[docs] def SetLocationPointGuidance(self, SimBase): """ Defines the Earth location pointing guidance module. """ self.locPoint.pHat_B = [1, 0, 0] self.locPoint.scAttInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg) self.locPoint.scTransInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg) self.locPoint.locationInMsg.subscribeTo(SimBase.EnvModel.groundStation.currentGroundStateOutMsg) messaging.AttRefMsg_C_addAuthor(self.locPoint.attRefOutMsg, self.attRefMsg)
[docs] def SetSpacecraftOrbitReconfig(self, SimBase): """ Defines the station keeping module. """ self.spacecraftReconfig.deputyTransInMsg.subscribeTo( SimBase.DynModels[self.spacecraftIndex].simpleNavObject.transOutMsg) self.spacecraftReconfig.attRefInMsg.subscribeTo(self.attRefMsg) self.spacecraftReconfig.thrustConfigInMsg.subscribeTo(self.fswThrusterConfigMsg) self.spacecraftReconfig.vehicleConfigInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleMassPropsObject.vehicleConfigOutMsg) self.spacecraftReconfig.mu = SimBase.EnvModel.mu # [m^3/s^2] self.spacecraftReconfig.attControlTime = 400 # [s] messaging.AttRefMsg_C_addAuthor(self.spacecraftReconfig.attRefOutMsg, self.attRefMsg) # connect a blank chief message chiefData = messaging.NavTransMsgPayload() chiefMsg = messaging.NavTransMsg().write(chiefData) self.spacecraftReconfig.chiefTransInMsg.subscribeTo(chiefMsg)
[docs] def SetAttitudeTrackingError(self, SimBase): """ Defines the module that converts a reference message into a guidance message. """ self.trackingError.attNavInMsg.subscribeTo( SimBase.DynModels[self.spacecraftIndex].simpleNavObject.attOutMsg) self.trackingError.attRefInMsg.subscribeTo(self.attRefMsg) messaging.AttGuidMsg_C_addAuthor(self.trackingError.attGuidOutMsg, self.attGuidMsg)
[docs] def SetMRPFeedbackRWA(self, SimBase): """ Defines the control properties. """ self.decayTime = 50 self.xi = 0.9 self.mrpFeedbackRWs.Ki = -1 # make value negative to turn off integral feedback self.mrpFeedbackRWs.P = 2 * np.max(SimBase.DynModels[self.spacecraftIndex].I_sc) / self.decayTime self.mrpFeedbackRWs.K = (self.mrpFeedbackRWs.P / self.xi) * \ (self.mrpFeedbackRWs.P / self.xi) / np.max( SimBase.DynModels[self.spacecraftIndex].I_sc) self.mrpFeedbackRWs.integralLimit = 2. / self.mrpFeedbackRWs.Ki * 0.1 self.mrpFeedbackRWs.vehConfigInMsg.subscribeTo(SimBase.DynModels[self.spacecraftIndex].simpleMassPropsObject.vehicleConfigOutMsg) self.mrpFeedbackRWs.rwSpeedsInMsg.subscribeTo( SimBase.DynModels[self.spacecraftIndex].rwStateEffector.rwSpeedOutMsg) self.mrpFeedbackRWs.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg) self.mrpFeedbackRWs.guidInMsg.subscribeTo(self.attGuidMsg)
[docs] def SetRWConfigMsg(self, SimBase): """ Imports the RWs configuration information. """ # Configure RW pyramid exactly as it is in the Dynamics (i.e. FSW with perfect knowledge) # the same msg is used here for both spacecraft self.fswRwConfigMsg = SimBase.DynModels[self.spacecraftIndex].rwFactory.getConfigMessage()
[docs] def SetThrustersConfigMsg(self, SimBase): """ Imports the thrusters configuration information. """ fswSetupThrusters.clearSetup() for key, th in SimBase.DynModels[self.spacecraftIndex].thrusterFactory.thrusterList.items(): loc_B_tmp = list(itertools.chain.from_iterable(th.thrLoc_B)) dir_B_tmp = list(itertools.chain.from_iterable(th.thrDir_B)) fswSetupThrusters.create(loc_B_tmp, dir_B_tmp, th.MaxThrust) self.fswThrusterConfigMsg = fswSetupThrusters.writeConfigMessage()
[docs] def SetRWMotorTorque(self): """ Defines the motor torque from the control law. """ controlAxes_B = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] self.rwMotorTorque.controlAxes_B = controlAxes_B self.rwMotorTorque.vehControlInMsg.subscribeTo(self.mrpFeedbackRWs.cmdTorqueOutMsg) self.rwMotorTorque.rwParamsInMsg.subscribeTo(self.fswRwConfigMsg)
# Global call to initialize every module
[docs] def InitAllFSWObjects(self, SimBase): """ Initializes all FSW objects. """ self.SetInertial3DPointGuidance() self.SetSunPointGuidance(SimBase) self.SetLocationPointGuidance(SimBase) self.SetAttitudeTrackingError(SimBase) self.SetRWConfigMsg(SimBase) self.SetThrustersConfigMsg(SimBase) self.SetMRPFeedbackRWA(SimBase) self.SetSpacecraftOrbitReconfig(SimBase) self.SetRWMotorTorque()
[docs] def setupGatewayMsgs(self, SimBase): """create C-wrapped gateway messages such that different modules can write to this message and provide a common input msg for down-stream modules""" self.attRefMsg = messaging.AttRefMsg_C() self.attGuidMsg = messaging.AttGuidMsg_C() self.zeroGateWayMsgs() # connect gateway FSW effector command msgs with the dynamics SimBase.DynModels[self.spacecraftIndex].rwStateEffector.rwMotorCmdInMsg.subscribeTo( self.rwMotorTorque.rwMotorTorqueOutMsg) SimBase.DynModels[self.spacecraftIndex].thrusterDynamicEffector.cmdsInMsg.subscribeTo( self.spacecraftReconfig.onTimeOutMsg)
[docs] def zeroGateWayMsgs(self): """Zero all the FSW gateway message payloads""" self.attRefMsg.write(messaging.AttRefMsgPayload()) self.attGuidMsg.write(messaging.AttGuidMsgPayload())