The acquisition sequence for the ComStar satellite.

Comstar is spinning in transfer orbit. When the demonstration starts it is at orbit normal. The acquisition sequence has the following phases:

1. Despin to the DST angular rate
2. DST with nutation damping
3. Pitch estimation
4. Pitch acquisition
5. Normal mode

Phases 2-4 make use of the tach loop

Since version 2.
-------------------------------------------------------------------------
See also DSTCrit, PtchLoop, SSA, SpinREst, StatePrp, FGs, NPlot,
ComStar, PitchEst, Q2Eul, Constant, Plot2D, TimeGUI, RK4
-------------------------------------------------------------------------

Contents

%-------------------------------------------------------------------------------
%	 Copyright 1994-1998 Princeton Satellite Systems, Inc. All rights reserved.
%-------------------------------------------------------------------------------

Global for the TimeGUI

%------------------------
global simulationAction
simulationAction = ' ';

Constants

%----------
rPSToRPM   = Constant('Rad/Sec to RPM');
rPMToRPS   = Constant('RPM to Rad/Sec');
degToRad   = Constant('Deg to Rad');
radToDeg   = Constant('Rad to Deg');

Database inputs

%----------------
inrMWA     = ComStar('MWA Inertia');
inr        = ComStar('TO Inertia');
wMWATarg   = ComStar('DST MWA Target');
uMWA       = ComStar('U MWA');
uDamper    = ComStar('U Damper');
cDamper    = ComStar('C Damper');
inrDamper  = ComStar('Damper Inertia');

invInr     = inv(inr);
hMWA       = inrMWA*wMWATarg;

Input parameters

%-----------------
w          = [0;0;4.5]*rPMToRPS;
nSim       = 4000;
nPMax      = 10;
dTSim      = 0.25;                  % Control period
x          = [[1;0;0;0];w;0;0];
esaLimit   = 8.0*degToRad;

The acquisition sequence has four phases:

%------------------------------------------
% 1. Despin to the DST angular rate
% 2. DST with nutation damping
% 3. Pitch estimation
% 4. Pitch acquisition
% 5. Normal mode
% Phases 2-4 make use of the tach loop

Compute an acceptable spin rate prior to the dual spin turn

%------------------------------------------------------------
[wZMax,wZMin,wYMax] = DSTCrit(inrMWA,inr(2,2),inr(1,1),inr(3,3),hMWA);

Initialize the Spin rate controller

%------------------------------------
wC             = wZMin + 0.2*(wZMax-wZMin);
wY             = -(wC*inr(3,3)-hMWA)/inr(2,2);
phase          = 'Despin';
xSpinRateEst   = x(7);
kSpinRateEst   = 0.5;
pulsewidth     = 0;
pulsewidthMax  = 0;
accelNom       = -6/inr(3,3);  % Need to update this
wError         = 0.01*rPMToRPS;
fprintf('Minimum pre  DST z-axis spin rate         = %8.2f rpm \n',wZMin*rPSToRPM);
fprintf('Maximum pre  DST z-axis spin rate         = %8.2f rpm \n',wZMax*rPSToRPM);
fprintf('Maximum post DST y-axis spin rate         = %8.2f rpm \n',wYMax*rPSToRPM);
fprintf('Pre  DST z-axis spin rate                 = %8.2f rpm \n',wC*rPSToRPM);
fprintf('Post DST y-axis spin rate                 = %8.2f rpm \n',wY*rPSToRPM);
Minimum pre  DST z-axis spin rate         =     0.45 rpm 
Maximum pre  DST z-axis spin rate         =     1.80 rpm 
Maximum post DST y-axis spin rate         =     1.80 rpm 
Pre  DST z-axis spin rate                 =     0.72 rpm 
Post DST y-axis spin rate                 =    -0.36 rpm 

Design the tach loop

%---------------------
zeta         = 0.7071;
wN           = 1.0;
tSamp        = 0.25;
zetaPL       = 0.7071;
wNPL         = 0.1;
beta         = 0.0;
fprintf('Tach loop damping ratio                   = %8.2f         \n',  zeta);
fprintf('Tach loop undamped natural freq           = %8.2f rad/sec \n',    wN);
fprintf('Tach loop sampling period                 = %8.2f sec     \n', tSamp);
fprintf('Pitch loop damping ratio                  = %8.2f         \n',zetaPL);
fprintf('Pitch loop undamped natural freq          = %8.2f rad/sec \n',  wNPL);
Tach loop damping ratio                   =     0.71         
Tach loop undamped natural freq           =     1.00 rad/sec 
Tach loop sampling period                 =     0.25 sec     
Pitch loop damping ratio                  =     0.71         
Pitch loop undamped natural freq          =     0.10 rad/sec 

Get the pitch and tach loop controllers

%----------------------------------------
[aM,bM,cM,dM,aP,bP,cP,dP] = PitchLoop(inr(2,2),inrMWA,beta,zeta,zetaPL,wN,wNPL,tSamp,'Delta');

Set up the pitch Kalman Filter

%-------------------------------
pPitchEst            = [pi^2 0;0 (0.1*pi/30)^2];
xPitchEst            = zeros(2,1);
rPitchEst            = (0.01*degToRad)^2;
qPitchEst            = (6*0.01)^2; % in-lbf 1 percent thruster torque uncertainty
esaPitchValid        = 8*degToRad;
covarianceThreshold  = 4.0;

Design the thruster pitch controller

%-------------------------------------
zetaTPL = 0.7071;
wNTPL   = 0.02;
fprintf('Thruster Pitch loop damping ratio         = %8.2f         \n',zetaTPL);
fprintf('Thruster Pitch loop undamped natural freq = %8.2f rad/sec \n',  wNTPL);

kTPL           = inr(2,2)*wNTPL^2;
tauTPL         = 2*zetaTPL/wNTPL;
pitchThreshold = 4*degToRad;
Thruster Pitch loop damping ratio         =     0.71         
Thruster Pitch loop undamped natural freq =     0.02 rad/sec 

The main simulation loop

%-------------------------
nPlot         = nSim/nPMax;
xPlot         = zeros(9,nPlot);
xPlotEst      = zeros(1,nPlot);
esaPitchPlot  = zeros(1,nPlot);
phasePlot     = zeros(1,nPlot);
tPlot         = zeros(1,nPlot);
xPlotPitchEst = zeros(2,nPlot);
pPlotPitchEst = zeros(2,nPlot);
thrusterPlot  = zeros(3,nPlot);
plotPitch     = zeros(1,nPlot);
t             = 0;
nP            = 0;
kP            = 0;
uSun          = [1;0;0];
uSunBodyOld   = uSun;
TLast         = -1;
T             = 2*pi/x(7);
tThruster     = [0;0;0];
pitchSat      = 0;
e             = [0;0;0];
pulseLeft     = 0;

phases        =  ['Despin        ';...
                  'Dual Spin Turn';...
                  'Estimate Pitch';...
                  'Acquire Pitch '];

Initialize the time display

%----------------------------
tToGoMem.lastJD        = 0;
tToGoMem.lastStepsDone = 0;
tToGoMem.kAve          = 0;
[ ratioRealTime, tToGoMem ] =  TimeGUI( nSim, 0, tToGoMem, 0, dTSim, 'ComStarAcq' );

for k = 1:nSim;

  % Display the status message
  %---------------------------
  [ ratioRealTime, tToGoMem ] = TimeGUI( nSim, k, tToGoMem, ratioRealTime, dTSim );

  % Sun sensor
  %-----------
  [cEP,dTCEP,uSunBodyOld] = SSA(x(1:4),uSun,uSunBodyOld,dTSim);

  % The gyro
  % --------
  yawRate = x(7);

  % The tachometer
  % --------------
  wSpeed = x(8);

  % Earth sensor
  %-------------
  e          = Q2Eul(x(1:4),e);
  pitch      = -e(3);
  if abs(pitch) > esaLimit & pitchSat == 0,
    esaPitch = sign(pitch)*esaLimit;
    pitchSat = 1;
  elseif abs(pitch) < esaLimit,
    esaPitch = pitch;
    pitchSat = 0;
  end

  % Store plot data
  %----------------
  if nP == 0,
    kP                  = kP + 1;
    xPlot(:,kP)         = x;
    tPlot(kP)           = t;
    xPlotEst(kP)        = xSpinRateEst;
    xPlotPitchEst(:,kP) = xPitchEst;
    pPlotPitchEst(:,kP) = [pPitchEst(1,1);pPitchEst(2,2)];
    thrusterPlot (:,kP) = tThruster;
    esaPitchPlot(kP)    = esaPitch;
    plotPitch(kP)       = pitch;
    if     ( strcmp(phase,'Despin'        ) == 1 ),
      phasePlot(kP) = 1;
    elseif ( strcmp(phase,'Dual Spin Turn') == 1 ),
      phasePlot(kP) = 2;
    elseif ( strcmp(phase,'Estimate Pitch') == 1 ),
      phasePlot(kP) = 3;
    else
      phasePlot(kP) = 4;
    end
	nP = nPMax - 1;
  else
    nP = nP - 1;
  end

  %-----------------------------------
  % The controller
  %-----------------------------------

  % Phase transition control
  %-------------------------
  if( strcmp(phase,'Despin') == 1 )

    if( abs(xSpinRateEst - wC) < wError )
      phase     = 'Dual Spin Turn';
      xTachLoop = 0;
      wYawOld   = yawRate;
      wYawMax   = 0;
      wNutLimit = 2*degToRad;
    end

  elseif( strcmp(phase,'Dual Spin Turn') == 1 )

    wYawMax = max(wYawMax,abs(yawRate));
    if yawRate*wYawOld < 0,
      if wYawMax < wNutLimit,
        phase  = 'Estimate Pitch';
      end
      wYawMax = 0;
    end
  	wYawOld  = yawRate;

  elseif ( strcmp(phase,'Estimate Pitch') == 1 ),

    if( pPitchEst(1,1) < covarianceThreshold ),
      phase = 'Acquire Pitch';
    end

  end

  % Control System
  %---------------
  if( strcmp(phase,'Despin') == 1 )
    tMWA = 0;
    if( cEP == 1 )
	  if( TLast ~= -1 )
	    T             = t - dTCEP - TLast;
      xSpinRateEst  = SpinREst(accelNom, kSpinRateEst, xSpinRateEst, T, pulsewidth);
      pulsewidthMax = abs(2*pi/xSpinRateEst);
	    rateError     = wC-xSpinRateEst;
      pulsewidth    = min(pulsewidthMax,abs(rateError/accelNom));
	    tThruster     = sign(rateError)*[0;0;6];
	    pulseLeft     = pulsewidth - dTSim;
	  end
	  TLast = t - dTCEP;
	else
	  pulseLeft     = pulseLeft - dTSim;
	end

	if ( pulseLeft < 0),
	  tThruster = [0;0;0];
	end

  elseif ( strcmp(phase,'Dual Spin Turn') == 1 ),
	wSDemand = wMWATarg;

  elseif ( strcmp(phase,'Estimate Pitch') == 1 ),
    [xPitchEst,pPitchEst] = PitchEst(xPitchEst,pPitchEst,dTSim,inr(2,2),qPitchEst,rPitchEst,...
	                                    tThruster(2),esaPitch,esaPitchValid);

  elseif ( strcmp(phase,'Acquire Pitch') == 1 ),
    tThruster = [0;-kTPL*(tauTPL*xPitchEst(2) + xPitchEst(1));0];
    [xPitchEst,pPitchEst] = PitchEst(xPitchEst,pPitchEst,dTSim,inr(2,2),qPitchEst,rPitchEst,...
	                                    tThruster(2),esaPitch,esaPitchValid);

  end

  % Tach loop
  %----------
  if (strcmp(phase,'Despin') == 0),
    [tMWA,xTachLoop] = StatePrp(aM,bM,cM,dM,xTachLoop,[wSpeed; wSDemand],'Delta');
  else
    tMWA = 0;
  end

  %-----------------------------------
  % The simulation
  %-----------------------------------

  x = RK4('FGs',x,dTSim,t,inr,invInr,tThruster,[inrMWA inrDamper],[uMWA uDamper],[tMWA 0],[0 cDamper]);
  t = t + dTSim;

  % Time control
  %-------------
  switch simulationAction
    case 'pause'
      pause
      simulationAction = ' ';
    case 'stop'
      return;
    case 'plot'
      break;
  end

end

j = 1:kP;

tPlot = tPlot(j);

Plot2D(tPlot,xPlot(5:8,j),'Time (sec)',['X and Z Rates (rad/sec)';...
    'MWA Speed (rad/sec)    '; 'Y-Axis Rate (rad/sec)  '],'Body Rates',...
    'lin',['[1,3]';'4    ';'2    ']);

Plot2D(tPlot,xPlotEst(j)-xPlot(7,j),'Time (sec)',...
    'Spin Rate Estimator error (rad/sec)','Estimator');

Plot2D(tPlot,[xPlotPitchEst(1,j)-plotPitch(j);xPlotPitchEst(2,j)-xPlot(6,j)]*radToDeg,...
    'Time (sec)', ['Pitch Error (deg)         ';...
    'Pitch Rate Error (deg/sec)'],'Pitch Control');

Plot2D(tPlot,[esaPitchPlot(j);plotPitch(j);xPlotPitchEst(1,j)]*radToDeg,...
    'Time (sec)', ['ESAPitch (deg)'; 'Pitch (deg)   '; 'Estimate (deg)'],'Pitch');

Plot2D(tPlot,pPlotPitchEst(1:2,j),'Time (sec)', ['Pitch Covariance     ';...
    'Pitch Rate Covariance'],'Pitch Covariance');

Plot2D(tPlot,thrusterPlot(:,j),'Time (sec)','Thruster torque (inlb)','Thruster Torque');

NPlot(phases,phasePlot(j),tPlot,'Time (sec)','Phase')

TimeGUI('close');

%--------------------------------------
% PSS internal file version information
%--------------------------------------