APPENDIX C: TRAJECTORY GENERATION, HARDWARE IN THE LOOP SIMULATION, AND AUTO BALANCE SOFTWARE


/********************************************************************************
*										
*	con.c
*
*	A flight controller for Ranger NBV.
*
*	Written by Joe Graves August 1994
*										
*********************************************************************************/
#include <vxWorks.h>
#include <taskLib.h>
#include <math.h>
#include <stdio.h>
#include <asiports.h>
#include <pivecs.h>
#include <dynamics_jdg.h>
#include <rotation_jdg.h>
#include <vector_jdg.h>
#include <sys.h>
#include <nbv_cs_newer.h>
#include <flt_common.h>
#include <nbv_common.h>
#include "con.h"
#define ZPITCH	0.6
#define	YYAW	0.4
#define	Pi	3.14159265
#define ABS(X) ( (X) < 0 ? -(X) : (X) )
#define	Er(x)	(V(CON_Des_State, x)-V(EST_State, x))
#define ESC	27
/****************************************
*					*
*	Function Prototypes		*
*					*
*****************************************/
void		conTerminate( void);
Vector		conAttitudeControlLaw(Vector, Vector);
Vector		conVectorError(Vector, Vector);
Vector		conAttitudeAngleAxis(Scalar, Vector);
Vector		conGetHcPilot(void);
Vector		conGetDesState(Vector,Vector,Vector);
extern Vector	conControlLaw(Vector,Vector);
Vector		conThrustCommand(Vector,Vector);
Vector		conQuatPart(Vector);
Vector		conOmegaPart(Vector);
Vector		conDXDtMoment(Vector);
Vector		conDXDtRotation(Vector);
Vector		conDXDtTotal(Vector);
Vector		conNormalizeQuat(Vector);
Vector		conDeadband(Vector, Scalar);
Vector		conIntegrateQuat(Vector, Vector, Scalar, int);
/****************************************
*					*
*	Module Status Globals		*
*					*
*****************************************/
extern Module_State	SYS_Module_State[MAXIMUM_ALLOWED_MODULES];
/****************************************
*					*
*	Input Globals			*
*					*
*****************************************/
extern Vector	EST_Quat;
extern Vector	EST_Omega;
extern Vector	EST_State;
extern Vector	FLT_HC;
extern Vector	IMU_Data;
extern Vector	IMU_Angular_Rate;
extern Vector	EST_Omega;
extern Scalar	CON_Adapt;
int		CON_Mode;
int		CON_Traj_Mode;
int		CON_Roll_Step=0;
int		CON_Pitch_Step=0;
int		CON_Yaw_Step=0;
int		CON_Mode_Change;
int		CON_Gain_Change;
int		CON_Trajectory_Change;
Vector		CON_Proportional_Gain;
Vector		CON_Derivative_Gain;
Vector		CON_a;
Vector		CON_a_max;
Scalar		CON_Kd;
Scalar		CON_Lambda;
Scalar		CON_RollTrjMag1;
Scalar		CON_RollTrjOmega1;
Scalar		CON_RollTrjMag2;
Scalar		CON_RollTrjOmega2;
Scalar		CON_PitchTrjMag1;
Scalar		CON_PitchTrjOmega1;
Scalar		CON_PitchTrjMag2;
Scalar		CON_PitchTrjOmega2;
Scalar		CON_YawTrjMag1;
Scalar		CON_YawTrjOmega1;
Scalar		CON_YawTrjMag2;
Scalar		CON_YawTrjOmega2;
int		CON_Roll_Traj_On;
int		CON_Pitch_Traj_On;
int		CON_Yaw_Traj_On;
int		CON_Safe_Count;
int		CON_Auto_Balance;
int		CON_Auto_Balance_Count;
Vector		CON_Auto_Balance_Torque_Avg;
extern BC_Command	BCS_Cmd;
extern float		BCS_Pos[6];
/****************************************
*					*
*	Output Globals			*
*					*
*****************************************/
extern Vector	THR_Hand_Controllers;
Vector		CON_Des_Quat;
Vector		CON_Des_Omega;
Vector		CON_Trj_Omega;
Vector		CON_Trj_Alpha;
Vector		CON_Des_State;
Vector		CON_Quat_Err;
Vector		CON_Omega_Err;
Vector		CON_Tau;
int		CON_Simulate = FALSE;
/****************************************
*					*
*	Internal Globals		*
*					*
*****************************************/
static SEM_ID	CON_Sem;
Vector		CON_HC_Pilot;
Vector		CON_HC_Control;
Vector		CON_HC_Thrusters;
Vector		CON_Sim_Moment;
Scalar		CON_Roll_Time;
Scalar		CON_Pitch_Time;
Scalar		CON_Yaw_Time;
int		CON_Sim_Mode;
int		CON_Saturate;
/****************************************
*					*
*	External Globals		*
*					*
*****************************************/
int conMain( void)
{
Vector	wdes, grav, gdes;
int	cnt, i;
Scalar	s1,s2,s3,theT=0.;
	printf("flight controller started\n\r");
	cnt = 0;
	/* set to 50 Hz running rate */
	CON_Sem = semBCreate( SEM_Q_FIFO, SEM_EMPTY);
	sysTaskManage( CON_Sem, 50, 0);
	CON_Des_State = VectorInit(13,0.0,0.0,0.0,
				      0.0,0.0,0.0,
				      0.0,0.0,0.0,1.0,
				      0.0,0.0,0.0);
	CON_Des_Quat = conQuatPart(CON_Des_State);
	CON_Des_Omega = conOmegaPart(CON_Des_State);
	CON_Proportional_Gain = VectorInit(3,0.0,0.0,0.0);
	CON_Derivative_Gain = VectorInit(3,0.0,0.0,0.0);
	CON_Mode = 0;
	CON_Traj_Mode = 1;
	CON_Mode_Change = TRUE;
	CON_Gain_Change = TRUE;
	CON_Trajectory_Change = TRUE;
	
	CON_Simulate = FALSE;
	
	CON_Safe_Count=0;
	CON_Sim_Mode = 0;
	CON_a = VectorInit(12, 275.0, 0.0, 0.0, 
				    500.0, 0.0,
				         500.0,
			   330.0,1000.0,1000.0,
				   0.0,0.0,0.0);
	CON_a = VectorInit(12, 0.0, 0.0, 0.0, 
				    0.0, 0.0,
				         0.0,
			         0.0,0.0,0.0,
				   0.0,0.0,0.0);
/*
	CON_a = VectorInit(12, 3.0, 0.0, 0.0, 
				    5.0, 0.0,
				         5.0,
			   10.0,14.0,14.0,
				   0.0,0.0,0.0);
*/
	CON_a_max = VectorInit(12, 14.0, 0.0, 0.0, 
				    20.0, 0.0,
				         20.0,
			   60.0,100.0,100.0,
				   0.0,0.0,0.0);
	CON_Kd = 0.0;
	CON_Lambda = 0.0;
	CON_RollTrjMag1 = 0.0;
	CON_RollTrjOmega1 = 0.0;
	CON_RollTrjMag2 = 0.0;
	CON_RollTrjOmega2 = 0.0;
	CON_PitchTrjMag1 = 0.0;
	CON_PitchTrjOmega1 = 0.0;
	CON_PitchTrjMag2 = 0.0;
	CON_PitchTrjOmega2 = 0.0;
	CON_YawTrjMag1 = 0.0;
	CON_YawTrjOmega1 = 0.0;
	CON_YawTrjMag2 = 0.0;
	CON_YawTrjOmega2 = 0.0;
	CON_Roll_Traj_On = 0;
	CON_Pitch_Traj_On = 0;
	CON_Yaw_Traj_On = 0;
	CON_Roll_Time = 0.0;
	CON_Pitch_Time = 0.0;
	CON_Yaw_Time = 0.0;
	CON_Trj_Omega = VectorInit(3,0.0,0.0,0.0);
	CON_Trj_Alpha = VectorInit(3,0.0,0.0,0.0);
	
	CON_Saturate = 0;
	CON_Auto_Balance = 0;
	CON_Auto_Balance_Count = 0;
	SYS_Module_State[CON].Done = FALSE;
	SYS_Module_State[CON].Ready = TRUE;
	
	
	
	
/*********************Temporary Debug Inits*******************/
	
/*	
	CON_RollTrjMag1 = .3;
	CON_RollTrjOmega1 = 0.2;
	CON_RollTrjMag2 = 0.0;
	CON_RollTrjOmega2 = 0.0;
	CON_PitchTrjMag1 = 0.2;
	CON_PitchTrjOmega1 = 0.3;
	CON_PitchTrjMag2 = 0.0;
	CON_PitchTrjOmega2 = 0.0;
	CON_YawTrjMag1 = 0.25;
	CON_YawTrjOmega1 = 0.17;
	CON_YawTrjMag2 = 0.0;
	CON_YawTrjOmega2 = 0.0;
	CON_Roll_Traj_On = 1;
	CON_Pitch_Traj_On = 1;
	CON_Yaw_Traj_On = 1;
	CON_Roll_Time = 0.0;
	CON_Pitch_Time = 0.0;
	CON_Yaw_Time = 0.0;
	CON_Trj_Omega = VectorInit(3,0.0,0.0,0.0);
	CON_Trj_Alpha = VectorInit(3,0.0,0.0,0.0);
    CON_Roll_Traj_On = TRUE;
    CON_Pitch_Traj_On = TRUE;
    CON_Yaw_Traj_On = TRUE;
    CON_Mode = 3;
    CON_Kd = 10.0;
    CON_Lambda = 1.0;
    CON_Adapt = 0.0;
*/
	EST_State = VectorInit(13,0.0,0.0,0.0,
				  0.0,0.0,0.0,
				  0.0,0.0,0.0,1.0,
				  .1, 0.0, 0.0);
	
/*********************End Temporary Debug Inits*******************/
	
	
	printf("entering con main\n\r");
	
/***	Main loop	***/
	while( !SYS_Module_State[CON].Done){
		semTake( CON_Sem, WAIT_FOREVER);
		sysSetStatus(CON, MODULE_RUNNING);
/*
		if (theT>50.) CON_Adapt=5000.;
		theT+=DT;
*/
		if(CON_Safe_Count-- <= 0){
		    FLT_HC = VectorInit(6, 0., 0., 0., 0., 0., 0.);
		    CON_Safe_Count=0;
		}
		CON_HC_Pilot = conGetHcPilot();
		CON_Des_State = conGetDesState(CON_HC_Pilot, CON_Des_State, EST_State);
		CON_HC_Control = conControlLaw(CON_Des_State, EST_State);
		THR_Hand_Controllers = conThrustCommand(CON_HC_Pilot,CON_HC_Control);
		
		if(CON_Auto_Balance == 0) {
		    CON_Auto_Balance_Count = 0;
		    CON_Auto_Balance_Torque_Avg = VectorInit(3, 0., 0., 0.);
		} else {
		    CON_Auto_Balance_Torque_Avg = AddVV(CON_Auto_Balance_Torque_Avg, 
			VectorInit(3, V(CON_HC_Control, 4), 
				      V(CON_HC_Control, 5), 
				      V(CON_HC_Control, 6)));
		    CON_Auto_Balance_Count++;
		    if(CON_Auto_Balance_Count == 250){
			CON_Auto_Balance_Torque_Avg = ScalarMultSV((.5)*1./((Scalar)CON_Auto_Balance_Count), CON_Auto_Balance_Torque_Avg);
			for(i=1;i<4;i++){
			    if(V(CON_Auto_Balance_Torque_Avg, i)>.2) V(CON_Auto_Balance_Torque_Avg, i) = .2;
			    if(V(CON_Auto_Balance_Torque_Avg, i)<-.2) V(CON_Auto_Balance_Torque_Avg, i) = -.2;
			}
			if(CON_Auto_Balance == 1) {
			    BCS_Pos[1] += V(CON_Auto_Balance_Torque_Avg, 2);
			    BCS_Pos[3] += V(CON_Auto_Balance_Torque_Avg, 1);
			    BCS_Cmd.Cmd = 1;
			    BCS_Cmd.Flag  = TRUE;
			}
			if(CON_Auto_Balance == 2) {
			    BCS_Pos[1] -= V(CON_Auto_Balance_Torque_Avg, 3);
			    BCS_Pos[4] += V(CON_Auto_Balance_Torque_Avg, 1);
			    BCS_Cmd.Cmd = 1;
			    BCS_Cmd.Flag  = TRUE;
			}
			CON_Auto_Balance_Count = 0;
			CON_Auto_Balance_Torque_Avg = VectorInit(3, 0., 0., 0.);
		    }
		}		    
		/* THR_Hand_Controllers = CON_HC_Pilot; */
		if(CON_Simulate) {
		    EST_Quat = conQuatPart(EST_State);
		    CON_Sim_Moment=ScalarMultSV(0.9 ,VectorInit(3,V(THR_Hand_Controllers,4),V(THR_Hand_Controllers,5),V(THR_Hand_Controllers,6)));
		    CON_Sim_Mode = 0;
		    /*EST_State = RungeKutta(conDXDtTotal,EST_State,DT);
		    EST_State = conNormalizeQuat(EST_State);*/
/*PrintV(EST_State);printf("\r");*/
		}
		if(cnt++>100) {
		    printf("%6.2f %6.2f %6.2f %6.2f %6.2f %6.2f \n\r", (float)V(CON_a, 1), (float)V(CON_a, 4), (float)V(CON_a, 6), (float)V(CON_a, 7), (float)V(CON_a, 8), (float)V(CON_a, 9));
/*
		    PrintV(VectorInit(6, V(CON_a, 1), V(CON_a, 4), V(CON_a, 6), V(CON_a, 7), V(CON_a, 8), V(CON_a, 9)));printf("\r");
		    PrintS(	CON_RollTrjMag1);printf("\n\r");
		    PrintS(	CON_RollTrjOmega1);printf("\n\r");
		    s1 = CON_Lambda*Er(7)+Er(11);
		    s2 = CON_Lambda*Er(8)+Er(12);
		    s3 = CON_Lambda*Er(9)+Er(13);
		    PrintS(sqrt(s1*s1+s2*s2+s3*s3));
		    PrintV(VectorInit(3, V(THR_Hand_Controllers, 4), V(THR_Hand_Controllers, 5), V(THR_Hand_Controllers, 6)));printf("\r");
*/
		    cnt = 0;
		}
if(CON_Simulate)printf("hello1\n\r");
	}
	conTerminate();
	return(0);
}
/* conTerminate
 *
 *	Function used to end program operation in  "normal" way at the moment it
 *	only calls exit(), but in the future it should also make sure that any
 *	relevant data is saved or that the user is notfied of any errors, etc...
 */
void conTerminate( void)
{
	printf("conTerminate Called\n\r");
	sysTaskUnmanage( CON_Sem);
	semDelete( CON_Sem);
	sysSetStatus(CON, MODULE_DELETED);
	SYS_Module_State[CON].Ready = FALSE;
}
Vector	conAttitudeControlLaw(Vector qe, Vector we)
{
Vector	x;
	if(V(qe,4) < 0.0) ScalarMultSV(-1.0,qe);
	qe = conDeadband(qe,0.000001);
	
	V(x,1) = V(qe,1)*(-V(CON_Proportional_Gain,1))+V(we,1)*(V(CON_Derivative_Gain,1));
	V(x,2) = V(qe,2)*(-V(CON_Proportional_Gain,2))+V(we,2)*(V(CON_Derivative_Gain,2));
	V(x,3) = V(qe,3)*(-V(CON_Proportional_Gain,3))+V(we,3)*(V(CON_Derivative_Gain,3));
	Dim(x) = 3;
	return(x);
}
Vector	conVectorError(Vector v, Vector vd)
{
Vector	x, axis;
Scalar	angleErr;
/* Find the angle between the sensed gravity vector, and the calculated gravity vector */
	angleErr = AngleVV(v,vd);
/* Find the axis of rotation between the actual and desired gravity vectors */
	axis = CrossVV(vd,vd);
	if(MagnitudeV(axis) < 0.000001)  /* this prevents a division by zero error */
		axis = VectorInit(3,0.0,1.0,0.0); /* arbitrary axis of rotation */
	else
		axis = UnitV(axis);
/* Express the rotation from the actual to the desired gravity vector as a quaternion */
	x = conAttitudeAngleAxis(angleErr,axis);
	return(x);
}
Vector	conAttitudeAngleAxis(Scalar ang, Vector ax)
{
Vector	x;
	ax = UnitV(ax);
	x = UnitV(VectorInit(4,V(ax,1)*sin(ang/2.0),V(ax,2)*sin(ang/2.0),
					V(ax,3)*sin(ang/2.0),cos(ang/2.0)));
	return(x);
}
Vector	conGetHcPilot(void) {
Vector	wwPilot, xxPilot, hcPilot;
	/* get the hand controller values from the control station */
	xxPilot = VectorInit(3,V(FLT_HC,1),V(FLT_HC,2),V(FLT_HC,3));
	wwPilot = VectorInit(3,V(FLT_HC,4),V(FLT_HC,5),V(FLT_HC,6));
	hcPilot = VectorInit(6,V(xxPilot,1),V(xxPilot,2),V(xxPilot,3),V(wwPilot,1),V(wwPilot,2),V(wwPilot,3));
	return(hcPilot);
}
Vector	conGetDesState(Vector hcPilot, Vector desState, Vector estState){
Vector	wwPilot, xxPilot, q1, w1;
Scalar	ma, wa, mb, wb, t;
    /*PrintV(CON_Quat_Err);printf("\r");*/
	xxPilot = VectorInit(3,V(hcPilot,1),V(hcPilot,2),V(hcPilot,3));
	wwPilot = VectorInit(3,V(hcPilot,4),V(hcPilot,5),V(hcPilot,6));
	q1 = VectorInit(4,V(desState,7),V(desState,8),V(desState,9),V(desState,10));
	w1 = ScalarMultSV(0.2,VectorInit(3,V(hcPilot,4),V(hcPilot,5),V(hcPilot,6)));
	if(CON_Roll_Step != 0) {
		q1 = AddVV(q1, ScalarMultSV(1.0, DQuatDt(q1, VectorInit(3,((Scalar)(CON_Roll_Step))/180.0*Pi,0.0,0.0))));
		CON_Roll_Step = 0;
	}
	if(CON_Pitch_Step != 0) {
		q1 = AddVV(q1, ScalarMultSV(1.0, DQuatDt(q1, VectorInit(3,0.0,((Scalar)(CON_Pitch_Step))/180.0*Pi,0.0))));
		CON_Pitch_Step = 0;
	}
	if(CON_Yaw_Step != 0) {
		q1 = AddVV(q1, ScalarMultSV(1.0, DQuatDt(q1, VectorInit(3,0.0,0.0,((Scalar)(CON_Yaw_Step))/180.0*Pi))));
		CON_Yaw_Step = 0;
	}
	V(desState,7) = V(q1,1);
	V(desState,8) = V(q1,2);
	V(desState,9) = V(q1,3);
	V(desState,10)= V(q1,4);
	/* Open loop with latch */
	if(CON_Traj_Mode == 1) {
		if(MagnitudeV(wwPilot)>.1) {
			desState = estState;
		}
	}
	/* Resolved rate */
	if(CON_Traj_Mode == 2) {
		if(MagnitudeV(wwPilot)>.1) {
			q1 = VectorInit(4,V(desState,7),V(desState,8),V(desState,9),V(desState,10));
			w1 = ScalarMultSV(0.2,VectorInit(3,V(hcPilot,4),V(hcPilot,5),V(hcPilot,6)));
			/*if((CON_Roll_Traj_On)&&(V(hcPilot,4>.5)) w1 = VectorInit(3,.15,.12,.12);*/
			q1 = AddVV(q1, ScalarMultSV(DT, DQuatDt(q1, w1)));
			/*q1 = conIntegrateQuat(q1, w1, DT, CON_Trajectory_Change);*/
			CON_Trajectory_Change = FALSE;
						
			V(desState,7) = V(q1,1);
			V(desState,8) = V(q1,2);
			V(desState,9) = V(q1,3);
			V(desState,10)= V(q1,4);
			V(desState,11)= V(w1,1);
			V(desState,12)= V(w1,2);
			V(desState,13)= V(w1,3);
		}
		else {
			V(desState,11)= 0.0;
			V(desState,12)= 0.0;
			V(desState,13)= 0.0;
		}
	}
	/* Spacecraft dynamics */
	if(CON_Traj_Mode == 3) {
		if(MagnitudeV(wwPilot)>.1) CON_Sim_Moment=ScalarMultSV(0.1,wwPilot);
		else CON_Sim_Moment=VectorInit(3,0.0,0.0,0.0);
		CON_Sim_Mode = 1;
		desState = RungeKutta(conDXDtTotal,desState,DT);
		desState = conNormalizeQuat(desState);
	}
	/* Now calculate and add on trajectory waveforms */
	q1 = VectorInit(4,V(desState,7),V(desState,8),V(desState,9),V(desState,10));
	if(CON_Roll_Traj_On) {
		ma = CON_RollTrjMag1;
		wa = CON_RollTrjOmega1;
		mb = CON_RollTrjMag2;
		wb = CON_RollTrjOmega2;
		t = CON_Roll_Time;
		V(CON_Trj_Omega,1) = ( ma*wa*cos(wa*t) + mb*wb*cos(wb*t) );
		V(CON_Trj_Alpha,1) = -( ma*wa*wa*sin(wa*t) + mb*wb*wb*sin(wb*t) );
		
		CON_Roll_Time += DT;
	}
	else {
		V(CON_Trj_Omega,1) = 0.0;
		V(CON_Trj_Alpha,1) = 0.0;
		CON_Roll_Time = 0.0;
	}
	if(CON_Pitch_Traj_On) {
		ma = CON_PitchTrjMag1;
		wa = CON_PitchTrjOmega1;
		mb = CON_PitchTrjMag2;
		wb = CON_PitchTrjOmega2;
		t = CON_Pitch_Time;
		V(CON_Trj_Omega,2) = ( ma*wa*cos(wa*t) + mb*wb*cos(wb*t) );
		V(CON_Trj_Alpha,2) = -( ma*wa*wa*sin(wa*t) + mb*wb*wb*sin(wb*t) );
		CON_Pitch_Time += DT;		
	}
	else {
		V(CON_Trj_Omega,2) = 0.0;
		V(CON_Trj_Alpha,2) = 0.0;
		CON_Pitch_Time = 0.0;
	}
	if(CON_Yaw_Traj_On) {
		ma = CON_YawTrjMag1;
		wa = CON_YawTrjOmega1;
		mb = CON_YawTrjMag2;
		wb = CON_YawTrjOmega2;
		t = CON_Yaw_Time;
		V(CON_Trj_Omega,3) = ( ma*wa*cos(wa*t) + mb*wb*cos(wb*t) );
		V(CON_Trj_Alpha,3) = -( ma*wa*wa*sin(wa*t) + mb*wb*wb*sin(wb*t) );
		
		CON_Yaw_Time += DT;
	}
	else {
		V(CON_Trj_Omega,3) = 0.0;
		V(CON_Trj_Alpha,3) = 0.0;
		CON_Yaw_Time = 0.0;
	}
	/*PrintV(w1);printf("\r");*/
	q1 = AddVV(q1, ScalarMultSV(DT, DQuatDt(q1, CON_Trj_Omega)));
	w1 = AddVV(w1,CON_Trj_Omega);
	if(CON_Traj_Mode != 3) {
	    V(desState,7) = V(q1,1);
	    V(desState,8) = V(q1,2);
	    V(desState,9) = V(q1,3);
	    V(desState,10)= V(q1,4);
	    V(desState,11)= V(w1,1);
	    V(desState,12)= V(w1,2);
	    V(desState,13)= V(w1,3);
	}
	
	desState = conNormalizeQuat(desState);
	return(desState);
}
/*
Vector	conControlLawOld(Vector ds, Vector es){
Vector	eq,dq,ew,dw,werr,hcControlRot,hcControl;
	eq = VectorInit(4,V(es,7),V(es,8),V(es,9),V(es,10));	
	dq = VectorInit(4,V(ds,7),V(ds,8),V(ds,9),V(ds,10));
	CON_Des_Quat = dq;	
	ew = VectorInit(3,V(es,11),V(es,12),V(es,13));	
	dw = VectorInit(3,V(ds,11),V(ds,12),V(ds,13));	
	if(CON_Mode == 0) {
		werr=AddVV(dw,ScalarMultSV(-1.0,ew));
		CON_Omega_Err = werr;
		CON_Quat_Err = conQuatError(dq,eq);
		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);
		
	}
	if(CON_Mode != 1) {
		werr=AddVV(dw,ScalarMultSV(-1.0,ew));
		CON_Omega_Err = werr;
		CON_Quat_Err = conQuatError(dq,eq);
		hcControlRot = conAttitudeControlLaw(CON_Quat_Err,werr);
		CON_Tau = hcControlRot;
		hcControl = VectorInit(6,0.0,0.0,0.0,V(hcControlRot,1),V(hcControlRot,2),V(hcControlRot,3));
	}
	return(hcControl);
}
*/
Vector	conThrustCommand(Vector hcPilot, Vector hcControl) {
int	i;
matrix	m;
Vector	wwPilot, xxPilot, hcThrust;
Scalar	max;
	m = matrixInit(3,3, 	0.0, 	0.0, 	0.0,
				0.0, 	0.0, 	ZPITCH,
				0.0, 	-YYAW, 	0.0);
	xxPilot = VectorInit(3,V(hcPilot,1),V(hcPilot,2),V(hcPilot,3));
	wwPilot = VectorInit(3,V(hcPilot,4),V(hcPilot,5),V(hcPilot,6));
	if((CON_Traj_Mode == 2)||(CON_Traj_Mode == 3)) wwPilot = VectorInit(3,0.0,0.0,0.0);
	/* Add in pitch and yaw compensation */
	wwPilot = AddVV(wwPilot,MultMV(m,xxPilot));
	hcPilot = VectorInit(6,V(xxPilot,1),V(xxPilot,2),V(xxPilot,3),V(wwPilot,1),V(wwPilot,2),V(wwPilot,3));
	max = 1.0;
	/* If any element of the hcControl vector is greater than 1, set max equal to it */
	for(i=1;i<=6;i++) {
		if(V(hcControl,i)>1.0) max = V(hcControl,i);
		else {
			if(V(hcControl,i)<-1.0) max = -V(hcControl,i);
		}
	}
	hcControl = ScalarMultSV(1.0/max,hcControl);
	hcThrust = AddVV(hcPilot,hcControl);
	max = 1.0;
	CON_Saturate = 0;
	/* If any element of the hcThrust vector is greater than 1, set max equal to it */
	for(i=1;i<=6;i++) {
		if(V(hcThrust,i)>1.0) {
		    max = V(hcThrust,i);
		    CON_Saturate = 1;
		}
		else {
			if(V(hcThrust,i)<-1.0) {
			    max = -V(hcThrust,i);
			    CON_Saturate = 1;
			}
		}
	}
	return(ScalarMultSV(1.0/max,hcThrust));
}
Vector	conQuatPart(Vector v) {
Vector	x;
	return(VectorInit(4,V(v,7),V(v,8),V(v,9),V(v,10)));
}
Vector	conOmegaPart(Vector v) {
Vector	x;
	return(VectorInit(3,V(v,11),V(v,12),V(v,13)));
}
Vector	conDXDtTotal(Vector v)
{
Vector	x;
	x = conDXDtRotation(v);
	x = AddVV(x, conDXDtMoment(v));
	return(x);
}
Vector	conDXDtMoment(Vector vv)
{
Vector	ww, mm, tt;
Vector	xx;
matrix	In, ab_ro, Dr_ro_ro;
	
	if(CON_Sim_Mode == 0) {
	    In=matrixInit(3,3,8.0,0.0,0.0,
			      0.0,12.0,0.0,
			      0.0,0.0,12.0);
	    Dr_ro_ro = matrixInit(3,3,30.0,0.0,0.0,
				      0.0,50.0,0.0,
				      0.0,0.0,50.0); /* (Kg)(meters)^2 */	    
	}
	if(CON_Sim_Mode == 1) {
	    In=matrixInit(3,3,8.0,0.0,0.0,
			      0.0,12.0,0.0,
			      0.0,0.0,12.0);
	    Dr_ro_ro = matrixInit(3,3,0.0,0.0,0.0,
				      0.0,0.0,0.0,
				      0.0,0.0,0.0); /* (Kg)(meters)^2 */	    
	}
	xx = ZeroVectorS(13);
	tt = ZeroVectorS(3);
	ww = VectorInit(3,0.,0.,0.);
	mm = VectorInit(3,0.,0.,0.);
	ww = VectorInit(3,V(vv,11),V(vv,12),V(vv,13));
	mm = CON_Sim_Moment;
        ab_ro = matrixInit(3,3,ABS(V(vv,11)),0.0,0.0,
				 0.0,ABS(V(vv,12)),0.0,
				 0.0,0.0,ABS(V(vv,13)));
        mm = AddVV(mm,ScalarMultSV(-1.0,MultMV(Dr_ro_ro,MultMV(ab_ro,ww))));
	tt = DWDtMoment(mm,ww,In);
	V(xx,11) = V(tt,1);
	V(xx,12) = V(tt,2);
	V(xx,13) = V(tt,3);
	Dim(xx) = Dim(vv);
	return(xx);
}
Vector	conDXDtRotation(Vector v)
{
Vector	q1, w1, t1;
Vector	x;
	
	x = ZeroVectorS(13);
	q1 = VectorInit(4,V(v,7),V(v,8),V(v,9),V(v,10));
	w1 = VectorInit(3,V(v,11),V(v,12),V(v,13));
	t1 = DQuatDt(q1, w1);
	V(x,7) = V(t1,1);
	V(x,8) = V(t1,2);
	V(x,9) = V(t1,3);
	V(x,10) = V(t1,4);
	Dim(x) = Dim(v);
	return(x);
}	
Vector	conNormalizeQuat(Vector Xr) {
Vector	q1;
	q1=VectorInit(4,V(Xr,7),V(Xr,8),V(Xr,9),V(Xr,10));
	q1=ScalarMultSV(1.0/MagnitudeV(q1),q1);
	V(Xr,7)=V(q1,1);V(Xr,8)=V(q1,2);V(Xr,9)=V(q1,3);V(Xr,10)=V(q1,4);
	return(Xr);
}
Vector	conDeadband(Vector v, Scalar s) {
int	i;
	for(i=1;i<=4;i++) {
		if((V(v,i)<s)&&(V(v,i)>-s)) V(v,i)=0.0;
	}
	return(v);
}
Vector		conIntegrateQuat(Vector q, Vector w, Scalar dt, int new) {
static Vector	olddq;
Vector		dq;
   	dq = DQuatDt(q, w);
	if((Dim(olddq)==0)||(new==TRUE)){
   		q.n[0] += dt*dq.n[0];
   		q.n[1] += dt*dq.n[1];
   		q.n[2] += dt*dq.n[2];
   		q.n[3] += dt*dq.n[3];
	} else {
   		q.n[0] += .5*dt*(olddq.n[0]+dq.n[0]);
   		q.n[1] += .5*dt*(olddq.n[1]+dq.n[1]);
   		q.n[2] += .5*dt*(olddq.n[2]+dq.n[2]);
   		q.n[3] += .5*dt*(olddq.n[3]+dq.n[3]);
	}
	olddq=dq;
   
	return(q);
}
/* end of file */