APPENDIX B: CONTROL SOFTWARE


/******************************************************
*nlcon.c
*
* Nonlinear attitude control strategy for Ranger NBV
*
*	Created by Rob Sanner - May 1995
*
*	Modifications and integration into Ranger 
*	flight control software
*	Joe Graves - June 1995
*
*******************************************************/
#include <stdio.h>
#include <vector_jdg.h>
#include <rotation_jdg.h>
#include "con.h"
#define ABS(X) ( (X) < 0 ? -(X) : (X) )
#define SGN(X) ( (X) < 0 ? -1.0 : \
                                 ( (X) > 0 ? 1.0 : 0.0 ) )
#define ALPHA	0.8
#define BETA	0.2
#define TH_SC	1.0
/****************************************
*					*
*	Function Prototypes		*
*					*
*****************************************/
Vector	conQuatError(Vector, Vector);
Vector	SubVV(Vector, Vector);
float	conRz(void);
/****************************************
*					*
*	Input Globals			*
*					*
*****************************************/
extern int		CON_Mode;
extern Vector		CON_Des_Quat;
extern Vector		CON_Quat_Err;
extern Vector		CON_Omega_Err;
extern Vector		CON_Trj_Alpha;
extern Vector		CON_Tau;
extern Vector		CON_a;
extern Vector		CON_a_max;
extern Scalar		CON_Kd;
extern Scalar		CON_Lambda;
extern int		CON_Mode_Change;
extern int		CON_Gain_Change;
Scalar 			CON_Adapt;
extern int		CON_Simulate;
extern Vector		THR_Hand_Controllers;
/****************************************
*					*
*	Internal Globals		*
*					*
*****************************************/
Vector		CON_Simulate_Ang_Bias;
Vector		CON_Control_Ang_Vel;
Scalar		CON_Simulate_Ang_Noise;
Scalar		CON_AngFilter;
Scalar		CON_G;
extern	int	CON_Saturate;
static Vector unfilt_dwd, dwd, old_wd, old_filt, qt, et;
static Vector tempV1, tempV2, g, wbd, wt, wr, s, dqt, det, tnl, tpd, tau;
static Vector oldda, da, dwr, dqd, olddqd;
static Vector wav;
static matrix Y, Cib, Cbd;
static Scalar dz;
void conInitACL(Scalar dzone, Scalar gam) {
  unfilt_dwd = VectorInit(3,0.,0.,0.);
  dwd = VectorInit(3,0.,0.,0.);
  old_wd = VectorInit(3,0.,0.,0.);
  old_filt   = VectorInit(3,0.,0.,0.);
  qt  = VectorInit(4,0.,0.,0.);
  et = VectorInit(3,0.,0.,0.);
  dqt = VectorInit(4,0.,0.,0.);
  det = VectorInit(3,0.,0.,0.);
  wav = VectorInit(3,0.,0.,0.);
  CON_Control_Ang_Vel = VectorInit(3,0.,0.,0.);
  CON_AngFilter = 0.;
  
  g   = VectorInit(3,0.,0.,1.);
  wt  = VectorInit(3,0.,0.,0.);
  wbd = VectorInit(3,0.,0.,0.);
  wr  = VectorInit(3,0.,0.,0.);
  dwr = VectorInit(3,0.,0.,0.);
  s   = VectorInit(3,0.,0.,0.);
  dqd      = VectorInit(4,0.,0.,0.,1.);
  olddqd   = VectorInit(4,0.,0.,0.,1.);
  tau  = VectorInit(3,0.,0.,0.);
  tnl  = VectorInit(3,0.,0.,0.);
  tpd  = VectorInit(3,0.,0.,0.);
  da    = VectorInit(12, 0., 0., 0., 0., 0., 0.,
		       0., 0., 0., 0., 0., 0.);
  oldda    = VectorInit(12, 0., 0., 0., 0., 0., 0.,
		       0., 0., 0., 0., 0., 0.);
  Row(Y)   = 3;
  Col(Y) = 12;
  Cib = matrixInit(3,3, 1., 0., 0.,
		        0., 1., 0.,
		        0., 0., 1.);
  Cbd = matrixInit(3,3, 1., 0., 0.,
		        0., 1., 0.,
		        0., 0., 1.);
  tempV1  = VectorInit(3,0.,0.,0.);
  tempV1  = VectorInit(3,0.,0.,0.);
  dz = dzone;
  CON_Adapt = gam;
}
Vector conControlLaw(Vector ds, Vector es) {
Vector q, qd, w, wd;
Scalar	q4sign;
static float invMag, gam;
static float my_wd[3], my_dwd[3];
Vector	hcControl;
register i;
Scalar	lmd;
Scalar	tempKd;
	if(CON_Gain_Change==TRUE) {
		conInitACL(0.001, 0.0);
		CON_Gain_Change = FALSE;
		PrintV(CON_Tau);
		printf("gain change\n\r");
	}
	if(CON_Mode_Change==TRUE) {
		conInitACL(0.001, 0.0);
		CON_Mode_Change = FALSE;
	}
	q = VectorInit(4,V(es,7),V(es,8),V(es,9),V(es,10));	
	qd = VectorInit(4,V(ds,7),V(ds,8),V(ds,9),V(ds,10));
	CON_Des_Quat = qd;	
	w = VectorInit(3,V(es,11),V(es,12),V(es,13));
	/*CON_Simulate_Ang_Bias = VectorInit(3, 0.02, 0.06, -0.06);*/
	CON_Simulate_Ang_Bias = VectorInit(3, 0.0, 0.0, 0.0);
	CON_Simulate_Ang_Noise = .05;
	
	if(CON_Simulate) {
	    w = AddVV(w, CON_Simulate_Ang_Bias);
	    w = AddVV(w, VectorInit(3, (((Scalar)conRz()-.5)*CON_Simulate_Ang_Noise), 
				       (((Scalar)conRz()-.5)*CON_Simulate_Ang_Noise), 
				       (((Scalar)conRz()-.5)*CON_Simulate_Ang_Noise)));	
	}
	wav = AddVV(ScalarMultSV((1.-CON_AngFilter), w), ScalarMultSV(CON_AngFilter, wav));
	w = wav;
	CON_Control_Ang_Vel = w;
	
	wd = VectorInit(3,V(ds,11),V(ds,12),V(ds,13));	
	/* Low pass filter differenced wd to get dwd */
	if (DT!=0.) {
		unfilt_dwd = ScalarMultSV(1./DT, SubVV(wd, old_wd));
		dwd = AddVV( ScalarMultSV(ALPHA, old_filt), 
		        ScalarMultSV(BETA, unfilt_dwd));
		old_wd   = wd;
		old_filt = dwd;
	}
	dwd = CON_Trj_Alpha;
	if(CON_Mode == 0) {
		CON_Omega_Err=AddVV(wd,ScalarMultSV(-1.0,w));
		CON_Quat_Err = conQuatError(qd,q);
		
		CON_Tau = VectorInit(3,0.0,0.0,0.0);
		hcControl = VectorInit(6, 0.0,0.0,0.0,0.0,0.0,0.0);
		
	/*PrintV(q);printf("************q\r");
	PrintV(qd);printf("qd\r");*/
	}
	if(CON_Mode != 0) {
		/*PrintS(CON_Adapt);printf("\n\r");*/
   		/* Compute tracking errors, viz:
			qt = q * (-qd)
			Cbd = RotFromQuat(qt)
			wbd = Cbd * wd
			wt = w - wbd
			wr = wbd - lam*qt(0,3)
			s = w - wr
   		*/
   		qt = conQuatError(qd,q);
   		Cbd = TransposeM(RotFromQuat(qt));
   		wbd=MultMV(Cbd, wd);
   		wt = SubVV(w, wbd);  
		q4sign = SGN(V(qt,4));
/*		q4sign = 1.0; */
   		V(et,1) = V(qt,1)*q4sign;
   		V(et,2) = V(qt,2)*q4sign;
   		V(et,3) = V(qt,3)*q4sign;
		tempKd = CON_Kd;
		lmd = CON_Lambda*(1./(100.*MagnitudeV(et)+1));
		if (lmd < CON_Lambda/5.) {lmd = CON_Lambda/5.; /*tempKd = 500.*CON_Kd;printf("lambda = %f hello\n\r", (float)lmd);*/}
   		wr = SubVV(wbd, ScalarMultSV(lmd , et));
   		/*wr = SubVV(wbd, ScalarMultSV(CON_Lambda, et));*/
    		s = SubVV(w, wr);
   		/* Compute nonlinear control input, viz:
			dqt = QuatKin(qt, wt)
			dwr = -(Skew(wt)*Cbd)*wd + Cbd*dwd - lam*dqt(0,3)
			<set up Y>
			tau = -kd*s + Y*ah
   		*/
   
		tnl = VectorInit(3,0.0,0.0,0.0); /* set the torque due to the nonlinear terms equal to zero */
   		if ((CON_Mode == 2) || (CON_Mode == 3)) {
			/* Compute current rotation matrix */
	
	   		Cib = RotFromQuat(q); 
	
	  		/* Determine gravity vector in current coords */
	    
	   		V(g,1) = M(Cib,3,1);              
	   		V(g,2) = M(Cib,3,2);
	   		V(g,3) = M(Cib,3,3);
	     		V(g,1) = 0.;
	     		V(g,2) = 0.;
	     		V(g,3) = 0.;
	
	     		dqt = DQuatDt(qt, wt);
	     		V(det,1) = V(dqt,1);
	     		V(det,2) = V(dqt,2);
	     		V(det,3) = V(dqt,3);
	     
	     		/*tempV1 = SubVV(MultMV(Cbd, dwd), ScalarMultSV(CON_Lambda, det));*/
	     		tempV1 = SubVV(MultMV(Cbd, dwd), ScalarMultSV(CON_Lambda*.01, det));
	     		/*tempV1 = SubVV(MultMV(Cbd, dwd), ScalarMultSV(CON_Lambda*.1, det));*/
	     		tempV2 = CrossVV(wt, wbd);
	
	     		dwr = SubVV(tempV1, tempV2);
	
	
	   		/* Fill the Y matrix  */
	
	     		Y.n[0][0] = dwr.n[0];
	     		Y.n[0][1] = dwr.n[1] - wr.n[2]*w.n[0];
	     		Y.n[0][2] = dwr.n[2] + wr.n[1]*w.n[0];
	     		Y.n[0][3] = -wr.n[2]*w.n[1];
	     		Y.n[0][4] = wr.n[1]*w.n[1]-wr.n[2]*w.n[2];
	     		Y.n[0][5] = wr.n[1]*w.n[2];
	     		Y.n[0][6] = wr.n[0]*fabs(wr.n[0]);
	     		Y.n[0][7] = Y.n[0][8]= Y.n[0][9] = 0.0;
	     		Y.n[0][10] = g.n[2];
	     		Y.n[0][11] = -g.n[1];
	
	     		Y.n[1][0] = wr.n[2]*w.n[0];
	     		Y.n[1][1] = dwr.n[0] + wr.n[2]*w.n[1];
	     		Y.n[1][2] = wr.n[2]*w.n[2] - wr.n[0]*w.n[0];
	     		Y.n[1][3] = dwr.n[1];
	     		Y.n[1][4] = dwr.n[2] - wr.n[0]*w.n[1];
	     		Y.n[1][5] = -wr.n[0]*w.n[2];
	     		Y.n[1][6] = 0.0;
	     		Y.n[1][7] = wr.n[1] * fabs(wr.n[1]);
	     		Y.n[1][8] = 0.0;
	     		Y.n[1][9] = -g.n[2];
	     		Y.n[1][10] = 0.0;
	     		Y.n[1][11] = g.n[0];
	
	     		Y.n[2][0] = -wr.n[1]*w.n[0];
	     		Y.n[2][1] = wr.n[0]*w.n[0] - wr.n[1]*w.n[1];
	     		Y.n[2][2] = dwr.n[0] - wr.n[1]*w.n[2];
	     		Y.n[2][3] = wr.n[0]*w.n[1];
	     		Y.n[2][4] = dwr.n[1] + wr.n[0]*w.n[2];
	     		Y.n[2][5] = dwr.n[2];
	     		Y.n[2][6] = Y.n[2][7] = 0.0;
	     		Y.n[2][8] = wr.n[2] * fabs(wr.n[2]);
	     		Y.n[2][9] = g.n[1];
	     		Y.n[2][10] = -g.n[0];
	     		Y.n[2][11] = 0.0;
	
	     		tnl = MultMV(Y, CON_a);
	
   		}
	
		/*PrintV(CON_a);*/
	
   		tpd = ScalarMultSV(-tempKd, s);
   		tau = AddVV(tpd, tnl);        
   		/* tau now holds the desired thruster torques in N-m */
	
		/* Convert tau to Ranger thruster commands */
	
		/*
   		for(i=0; i<3; i++) {
			Torque[i]=(short) (SGN(tau[i])*floor((10+fabs(tau[i]))/20.)); 
			if (Torque[i]>15)  Torque[i] = 15;
			if (Torque[i]<-15) Torque[i] = -15;
	   	}   
			*/
	   
   		/* Propagate controller states using trapezoidal method */
   		if (CON_Mode == 3) {
  
   			if(MagnitudeV(s)> dz) {
   				da = MultMV(TransposeM(Y), s);
   				gam = CON_Adapt;
    				gam = CON_Adapt*(1./(10.*MagnitudeV(s)+1.));
 			} else gam = 0.;
			/*gam = gam * (Scalar)(1-CON_Saturate);*/
			if(MagnitudeV(CON_Tau)>MagnitudeV(THR_Hand_Controllers)) gam = 0.;
	    		for (i=0; i<12; i++) da.n[i] *= -gam;
	    		for(i=0; i<12; i++) CON_a.n[i]+= .5*DT*(oldda.n[i]+da.n[i]);
	    		for(i=0; i<12; i++) if(CON_a.n[i]>CON_a_max.n[i]) CON_a.n[i]=CON_a_max.n[i];
	    		for(i=0; i<12; i++) if(CON_a.n[i]<0.) CON_a.n[i]=0.;
	    		for(i=0; i<12; i++) oldda.n[i]=da.n[i]; 
  		}
   		/* Brute-force quaternion normalization */
   		invMag=1./sqrt(qd.n[0]*qd.n[0]+qd.n[1]*qd.n[1]
				  +qd.n[2]*qd.n[2]+qd.n[3]*qd.n[3]);
     
   		qd.n[0] *= invMag;
   		qd.n[1] *= invMag;
   		qd.n[2] *= invMag;
   		qd.n[3] *= invMag;        
		hcControl = VectorInit(6,0.0,0.0,0.0,V(tau,1)*TH_SC,V(tau,2)*TH_SC,V(tau,3)*TH_SC);
		CON_Omega_Err = wt;
		CON_Quat_Err = qt;
		CON_Tau = tau;
	}
	return(hcControl);
}
Vector	conQuatError(Vector q, Vector qd)
{
Vector	x;
	V(q,1) = - V(q,1);
	V(q,2) = - V(q,2);
	V(q,3) = - V(q,3);
	V(x,1) =   V(q,4)*V(qd,1) - V(q,3)*V(qd,2) + V(q,2)*V(qd,3) + V(q,1)*V(qd,4);
	V(x,2) =   V(q,3)*V(qd,1) + V(q,4)*V(qd,2) - V(q,1)*V(qd,3) + V(q,2)*V(qd,4);
	V(x,3) = - V(q,2)*V(qd,1) + V(q,1)*V(qd,2) + V(q,4)*V(qd,3) + V(q,3)*V(qd,4);
	V(x,4) = - V(q,1)*V(qd,1) - V(q,2)*V(qd,2) - V(q,3)*V(qd,3) + V(q,4)*V(qd,4);
	
	Dim(x) = 4;
	return(x);
}
/*	SubVV:
 	This command adds two vectors, and returns the sum.
 	If the two vectors are not equidimensional an empty vector (Dim(v)=0) is returned
*/
Vector	SubVV(Vector a, Vector b ) 					
{
	Vector x;
	unsigned int cnt = 0;
	
	if(Dim(a) == Dim(b)) {
		while(cnt < Dim(a)) {
		x.n[cnt] = a.n[cnt] - b.n[cnt];
		cnt = cnt + 1;
		}
		Dim(x) = Dim(a);
	}
	else {
	  printf("Error in SubVV!\n");
	  Dim(x)=0;
	}
	
	return(x);
}
float	conRz(void) {
	return((float)(rand()/32768.0));
}
/* end of file */