APPENDIX E: TCU, BCU, IMU, AND ESU SOFTWARE

 

Main thruster module that handles jet select logic, and thruster scaling.


/********************************************************************************
*										
*	thr.c
*
*	This file contains the routines necessary for the initialization, control
*	and termination of the Ranger NBV thruster system
*
*	Written by Joe Graves August 1994
*										
*	10/21/94	Phil
*		Added a couple more printf's to help debug download failures.
*********************************************************************************/
#include <vxWorks.h>
#include <taskLib.h>
#include <math.h>
#include <pivecs.h>
#include <stdio.h>
#include <asiports.h>
#include "thr_mm.h"
#include "thr_shared.h"
#include <vector_jdg.h>
#include <sys.h>
#include <nbv_cs_newer.h>
#include <nbv_common.h>
#define ESC	27
#define	SCALE	0.6	/* this number indicates how much worse the thrusters perform in reverse as opposed
			to forward.  This is due to the fact that the propellers are optimized to work better
			in the forward direction.*/
/****************************************
*					*
*	Function Prototypes		*
*					*
*****************************************/
void		thrTerminate( void);
ChanPtr 	thrVxCommInit( void);
Scalar		thrNegScale(Scalar);
Scalar		thrPosScale(Scalar);
void		thrCalculateThrust(void);
void		thrSendThrust( int thruster);
extern void	fltSendThrusters(Vector);
/****************************************
*					*
*	Module Status Globals		*
*					*
*****************************************/
extern Module_State	SYS_Module_State[MAXIMUM_ALLOWED_MODULES];
/****************************************
*					*
*	Input Globals			*
*					*
*****************************************/
Vector		THR_Hand_Controllers;
Scalar		THR_Scale;
/****************************************
*					*
*	Output Globals			*
*					*
*****************************************/
long		THR_Speed[MAX_THRUSTERS];
long		THR_temp[MAX_THRUSTERS];
/****************************************
*					*
*	Internal Globals		*
*					*
*****************************************/
static SEM_ID	THR_Sem;
ChanPtr		THR_MMChannel;
ChanPtr		THR_pppp;
int		THR_Loop;
Vector		THR_SPD;
/****************************************
*					*
*	External Globals		*
*					*
*****************************************/
int thrMain( void)
{
int		count;
int		error = noErr;
Byte		data[7];
Byte		TheReset[2];
register	i;
    
	SYS_Module_State[THR].Done = FALSE;
	printf("Thruster Controller Started\n\r");
	/* set to 50 Hz running rate */
	THR_Sem = semBCreate( SEM_Q_FIFO, SEM_EMPTY);
	sysTaskManage( THR_Sem, 50, 0);
	THR_Scale = SCALE;
	THR_MMChannel = thrVxCommInit();
	THR_pppp=THR_MMChannel;
	pvHideMsgs( TRUE, THR_MMChannel);
	error = mmFileSend(2, THR_DOWNLOAD, THR_MMChannel);
	if( error){
		printf( "Error %d, Thruster MiMiCS not initialized\n\r", error);
		pvCloseChannel(THR_pppp);
		SYS_Module_State[THR].Ready = FALSE;
		SYS_Module_State[THR].Done = TRUE;
		return(ERROR);
		}
    data[0] = 2;
	TheReset[0] = PROCESSOR_1;
	for( i = 0; i <= (MAX_THRUSTERS-1) ; i++ ){
		TheReset[1] = i;
		printf("resetting lm629 #%d\n\r",i);
		taskDelay(1);
		pvSend( 0xA2, TheReset, 2, THR_MMChannel );
	}
	for( i = 0; i <= (MAX_THRUSTERS-1) ; i++ ){
		printf("Updating gains for thruster #%d\n\r",i);
		taskDelay(1);
		thrSendGain( i, KP, 10 );
	}
	for( i = 0; i <= (MAX_THRUSTERS-1) ; i++ ){
		printf("Updating gains for thruster #%d\n\r",i);
		taskDelay(1);
		thrSendGain( i, KD, 150 );
	}
	/* Kick off thruster firing loop */
			
	taskDelay(1);
	pvSend( 0xC9, NULL , NULL, THR_MMChannel );
	THR_Loop = TRUE;
	pvWorry2( THRUSTERROR, 0xC9, THR_MMChannel, 1 );
	printf("Thruster loop started\n\r");
    count = 0;
    SYS_Module_State[THR].Ready = TRUE;
/***	Main loop	***/
	while( !SYS_Module_State[THR].Done){
		semTake( THR_Sem, WAIT_FOREVER);
		sysSetStatus(THR, MODULE_RUNNING);
		thrCalculateThrust();
		if(count++ > 25) {
			count = 0;
			fltSendThrusters(THR_SPD); /* return thrust value to control station */
		}
	}
	thrTerminate();
	return(0);
}
ChanPtr
thrVxCommInit( void)
{
  ChanPtr	theChannel;
  theChannel = pvNewChannel();
  if( theChannel == 0){
	  printf("Null Pointer from pvNewChannel, #1\n\r");
	  thrTerminate();
	  }
  theChannel->port = THR_COM;
  if( mmOpenChannel( theChannel) != noErr){
	  printf("pvOpenChannel has failed for THR module\n\r");
	  }
  /* ok, now initialize the message handlers and schtuff */
  pvInitMsg( theChannel, lm629_Handlers, lm629_Msgs, lm629_HiPri ); 
  return( theChannel);
}
void	thrCalculateThrust (void)
{
long	DesSp[MAX_THRUSTERS];
float	MaxSpeed = 1500.0;  /* thruster maximum angular velocity (RPM) */
float	pitchSpeed=0.0, rollSpeed=0.0, yawSpeed=0.0;
float	xSpeed=0.0, ySpeed=0.0, zSpeed=0.0;
float	thrustPercent[8];
float	maxPercentage = 0.0, foo;
long	speedFactor;
int	i;
Vector	thc;
	if(SYS_Module_State[THR].StandBy == TRUE) 
		thc=VectorInit(6,0.0,0.0,0.0,0.0,0.0,0.0);
	else
		thc=THR_Hand_Controllers;
/*	PrintV(thc);printf("\r");*/
	
	xSpeed = V(thc,1);
	ySpeed = V(thc,2);
	zSpeed = V(thc,3);
	rollSpeed  = -V(thc,4);
	pitchSpeed = -V(thc,5);
	yawSpeed   = -V(thc,6);
		
	thrustPercent[0] = ( xSpeed + thrPosScale(yawSpeed)   + thrPosScale(pitchSpeed));
	thrustPercent[1] = ( xSpeed - thrNegScale(yawSpeed)   + thrPosScale(pitchSpeed));
	thrustPercent[2] = ( xSpeed + thrPosScale(yawSpeed)   - thrNegScale(pitchSpeed));
	thrustPercent[3] = ( xSpeed - thrNegScale(yawSpeed)   - thrNegScale(pitchSpeed));
	thrustPercent[4] = (-ySpeed + thrPosScale(rollSpeed) );
	thrustPercent[5] = (-ySpeed - thrNegScale(rollSpeed) );
	thrustPercent[6] = (-zSpeed + thrPosScale(rollSpeed) );
	thrustPercent[7] = (-zSpeed - thrNegScale(rollSpeed) );
	/*find out if any thrusters want to run faster than possible
	 * and pick out the largest value*/
	maxPercentage = 1.0;
	for( i = 0; i < MAX_THRUSTERS; i++ ) {
	    foo = fabs( thrustPercent[i]);
	    if( foo > maxPercentage )
		maxPercentage = foo;
	   /* printf(">%f< ", (float)thrustPercent[i]);*/
/*	    if( foo < 0.05)
		thrustPercent[i] = 0.0;*/
	}
	/* now scale all of the thrusters to accomodate any overthrust requests */
	speedFactor = (long)((MaxSpeed*573.33333));
	for( i = 0; i < MAX_THRUSTERS; i++ ) {
		DesSp[i] = 0.;
	/*
		if(thrustPercent[i]==0.){
		} else {
		    if(thrustPercent[i]>=0.){
			DesSp[i] = (long)(pow((float)(thrustPercent[i]/maxPercentage), .65) * speedFactor);
		    } else {
			DesSp[i] = -(long)(pow((float)(-thrustPercent[i]/maxPercentage), .65) * speedFactor);		    
		    }
		}
	*/
		if(thrustPercent[i]>=0.){
		    DesSp[i] = (long)((float)(thrustPercent[i]/maxPercentage) * speedFactor);
		} else {
		    DesSp[i] = -(long)((float)(-thrustPercent[i]/maxPercentage) * speedFactor);		    
		}
		/*printf(" P%dP", (int)thrustPercent[i]);
		printf(" %d", (int)DesSp[i]);*/
	
		/* DesSp[i] = 34400.0; */   /* sets all thrusters to 60 RPM for testing*/
			/* Now set a vector with the 8 desired thrust percentages to be sent back to the control station */
		V(THR_SPD,i+1)=thrustPercent[i];
		Dim(THR_SPD)=8;
	}
	/* This next section is designed to keep me from ripping off the propellers.  Essentially
		it sets a maximum acceleration for the thrusters.  (don't laugh.. the first time 
		we did attitude hold with Ranger we broke a prop off)
		later note... we broke many props off and finally had them replaced with metal ones 
		so that the max acceleration could be increased again.  */
	for( i = 0; i < MAX_THRUSTERS; i++ ) {
		THR_Speed[i] = DesSp[i];
/*
		if(THR_Speed[i] != DesSp[i]) {
			if(THR_Speed[i] < DesSp[i]) {
				THR_Speed[i] = THR_Speed[i] + MaxAccel;
				if(THR_Speed[i] > DesSp[i]) THR_Speed[i] = DesSp[i];
			}
			else 
			if(THR_Speed[i] > DesSp[i]) {
				THR_Speed[i] = THR_Speed[i] - MaxAccel;
				if(THR_Speed[i] < DesSp[i]) THR_Speed[i] = DesSp[i];
			}
		}
*/
	}
    
}
/* thrTerminate
 *
 *	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 thrTerminate( void)
{
int	i;
	printf("thrTerminate Called\n\r");
	if( SYS_Module_State[THR].Ready == FALSE)
		return;
	for( i = 0; i < 8; i++){
		THR_Speed[i]=0;
		thrSendThrust( i);
        }
	fltSendThrusters( VectorInit(8,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0));
	sysTaskUnmanage( THR_Sem);
	semDelete( THR_Sem);
    sysSetStatus(THR, MODULE_DELETED);
	SYS_Module_State[THR].Ready = FALSE;
	pvCloseChannel(THR_pppp);
}
Scalar thrNegScale(Scalar s1)
{
	if(s1 < 0.0) s1 = s1 * THR_Scale;
	return(s1);
}
Scalar thrPosScale(Scalar s1)
{
	if(s1 > 0.0) s1 = s1 * THR_Scale;
	return(s1);
}
/* end of file */

End Thruster Software
Begin BCS Software


 

/********************************************************************************
*										
*	bcs.c
*
*	This file contains the routines necessary for the initialization, control
*	and termination of the Ranger NBV buoyancy compensation system
*
*	Written by Joe Graves October 1994
*										
*	10/24/94	Phil 
*		Cleaned up comments and a few other things.
*********************************************************************************/
#include <vxWorks.h>
#include <taskLib.h>
#include <pivecs.h>
#include <stdio.h>
#include <asiports.h>
#include "bcs.h"
#include "sys.h"
#include "nbv_cs_newer.h"
#include "nbv_common.h"
#include "flt_common.h"
/****************************************
*					*
*	Function Prototypes		*
*					*
*****************************************/
void		bcsTerminate( void);
ChanPtr 	bcsVxCommInit( void);
/* int			bcsDoKeys(void); */ 
void	bcsCommand(BC_Command);
/****************************************
*					*
*	Module Status Globals		*
*					*
*****************************************/
 extern Module_State	SYS_Module_State[MAXIMUM_ALLOWED_MODULES];
/****************************************
*					*
*	Input Globals			*
*					*
*****************************************/
	/******************************************************************
	* BCS_Cmd
	*	-1 - Reset
	*	 0 - Reference wrt end of bc
	*	 1 - Set new BC position
	*******************************************************************/
BC_Command	BCS_Cmd;
float		BCS_Pos[6];
float		BCS_Act_Pos[6];
/****************************************
*					*
*	Output Globals			*
*					*
*****************************************/
/****************************************
*					*
*	Internal Globals		*
*					*
*****************************************/
static ChanPtr		BCS_Channel;
static int		BCS_done = FALSE;
static SEM_ID		BCS_Sem;
float			BCS_Length[6];
/****************************************
*					*
*	External Globals		*
*					*
*****************************************/
int
bcsMain( void)
{
  int		error = noErr;
  printf("Buoyancy Compensation System Controller Starting\n\r");
  /* set to 25 Hz running rate */
  BCS_Sem = semBCreate( SEM_Q_FIFO, SEM_EMPTY);
  sysTaskManage( BCS_Sem, 25, 0);
  BCS_Channel = bcsVxCommInit();
  pvInitMsg( BCS_Channel, bcsHandlers, bcsMsgs, bcsHiPri );
  pvHideMsgs( TRUE, BCS_Channel);
  SYS_Module_State[BCS].Ready = TRUE;
	/* taskDelay(1); */
  error = mmFileSend(2, BCS_DOWNLOAD, BCS_Channel);
  if( error != noErr){
	  bcsTerminate();
	  return ERROR;
	  }
	BCS_Length[0] = 25.0;
	BCS_Length[1] = 25.0;
	BCS_Length[2] = 14.0;
	BCS_Length[3] = 14.0;
	BCS_Length[4] = 26.0;
	BCS_Length[5] = 26.0;
  SYS_Module_State[BCS].Ready = TRUE;
  SYS_Module_State[BCS].Done = FALSE;
/***	Main loop	***/
	while( !SYS_Module_State[BCS].Done){
		semTake( BCS_Sem, WAIT_FOREVER);
		sysSetStatus(BCS, MODULE_RUNNING);
		/*	This needs to be ported over to tcx messages */
		/*BCS_done = bcsDoKeys();*/
		if(BCS_Cmd.Flag == TRUE){
			bcsCommand(BCS_Cmd);
			BCS_Cmd.Flag = FALSE;
		}
	}
	bcsTerminate();
	return(0);
}
ChanPtr	bcsVxCommInit( void)
{
  ChanPtr	theChannel;
  theChannel = pvNewChannel();
  if( theChannel == 0){
	  printf("bcs: Null Pointer from pvNewChannel, #1\n\r");
	  bcsTerminate();
	  }
  theChannel->port = BCS_COM;
  if( mmOpenChannel( theChannel) != noErr){
	  printf("bcs: mmOpenChannel has failed for BCS module\n\r");
	  }
  return( theChannel);
}
/* bcsTerminate
 *
 */
void bcsTerminate( void)
{
	sysTaskUnmanage( BCS_Sem);
	semDelete( BCS_Sem);
	SYS_Module_State[BCS].Ready = FALSE;
	pvCloseChannel(BCS_Channel);
	sysSetStatus(BCS, MODULE_DELETED);
	printf("bcsTerminate Called\n\r");
}
/* bcsDoKeys
 *
 *	a function which interprets and deals with keyDown events.  Lot's of
 *	junk here which is being used for debugging PiVeCS.
 *
 *	Returns:
 *		true or false to indicate whether the user asked to quit
 */
/*
int bcsDoKeys( void)
{
int		error = noErr;
int		done = FALSE;
int		i;
Byte	dumpMsg[5];
Byte	data[7], sum,CH;
static Byte MODE=0,RBC;
static int pos;
char	key;
	if( gfkbhit())
		key = getkey();
	else
		return FALSE;
	switch( key)
	  {
		case ESC:
			done = TRUE;
			break;
		case 'F':
			data[0]=2;
			 data[1]=RBC|0x10;
			 pvSend( 0xe2, data, 2, BCS_Channel);
			 break;
		case 'R':
               data[0]=2;
			 data[1]=RBC;
			 pvSend(0xe2 , data, 2 ,BCS_Channel);
			 break;
		case 'i':
			pos+=100;
			data[0]=2;
			data[1]=pos>>8;
			data[2]=pos & 0xff;
			data[3]=RBC;
			pvSend(0xcc,data,4,BCS_Channel);
			break;
		case 'I':
			pos-=100;
			data[0]=2;
			data[1]=pos>>8;
			data[2]=pos & 0xff;
			data[3]=RBC;
			pvSend(0xcc,data,4,BCS_Channel);
			break;
		case 't':
			pos+=10;
			data[0]=2;
			data[1]=pos>>8;
			data[2]=pos & 0xff;
			data[3]=RBC;
			pvSend(0xcc,data,4,BCS_Channel);
			break;
		case 'T':
			pos-=10;
			data[0]=2;
			data[1]=pos>>8 &0xff;
			data[2]=pos & 0xff;
			data[3]=RBC;
			pvSend(0xcc,data,4,BCS_Channel);
			break;
		case 'h':
			pos+=1;
			data[0]=2;
			data[1]=pos>>8;
			data[2]=pos & 0xff;
			data[3]=RBC;
			pvSend(0xcc,data,4,BCS_Channel);
			break;
		case 'H':
			pos-=1;
			data[0]=2;
			data[1]=pos>>8 &0xff;
			data[2]=pos & 0xff;
			data[3]=RBC;
			pvSend(0xcc,data,4,BCS_Channel);
			break;
		case 'x':
				RBC=0;
				break;
		case 'X':
				RBC=1;
				break;
		case 'y':
				RBC=2;
				break;
		case 'Y':
				RBC=3;
				break;
		case 'z':
				RBC=4;
				break;
		default:
			  break;
	 }
	 printf("\n\n\n\n\n\n\n\n\n\n\n\n\n\n %1c %5i ",RBC+48,pos);
	if (error != noErr)
		pvHandleError( error);
	else
		return done;
}
*/
void	bcsCommand(BC_Command x) {
Byte	RBC;
Byte	data[7];
int	i,pos;
/*	printf("do command\n\r");
	printf("joint %d   command %d\n\r",x.BC,x.Cmd);
*/
	if(x.Cmd == 1) {
		for(i=0;i<6;i++) {
			RBC = i;
 
			pos = (int)(100.0*BCS_Pos[i]*BCS_Length[i]/2.0);
			data[0]=2;
			data[1]=pos>>8;
			data[2]=pos & 0xff;
			data[3]=i;
			pvSend(0xcc,data,4,BCS_Channel);
			printf("bc[%d] to %3.2f in.\n\r", i, (float)pos/100.0);
		}
	}
	if(x.Cmd == 0) {
		RBC = (Byte)x.BC;
        	data[0]=2;
		data[1]=RBC;
		printf("reset for bc %d \n\r",(int)RBC);
		pvSend(0xe2 , data, 2 ,BCS_Channel);
	}
	if(x.Cmd == -1) {
		RBC = (Byte)x.BC;
         	data[0]=2;
		data[1]=RBC|0x10;;
		printf("find zero for bc %d \n\r",(int)RBC);
		pvSend( 0xe2, data, 2, BCS_Channel);
	}
}
void bcsShowDump( MsgPtr msg)
{
int	 addr;
static int	line = 5;
	addr = (msg->data[1] << 8) + msg->data[2];
/*
	printf( "%2d  %4X  %2X %2X %2X %2X\n",msg->data[0], addr, msg->data[3],
			msg->data[4], msg->data[5], msg->data[6]); */
	if( line > 20)
		line = 0;
	return;
}
/* end of file */

End BCS Software
Begin IMU Message Handler Software

/********************************************************************************
*										
*	imu_MiMiCS_handlers.c
*
*	This file contains the message handlers for the 
*	Ranger NBV Inertial Measurement Unit.
*
*	Written by Joe Graves August 1994
*										
*********************************************************************************/
#include <vxWorks.h>
#include <timers.h>
#include <pivecs.h>
#include <math.h>
#include <stdio.h>
#include "arm.msg"
#include <vector_jdg.h>
/****************************************
*					*
*	Globals				*
*					*
*****************************************/
extern	ChanPtr IMU_MMChannel;
extern	Vector	IMU_Data;
extern Vector	IMU_Angular_Rate_Bias;
extern Vector	IMU_Angular_Rate;
extern Vector	IMU_Acceleration;
extern Vector	IMU_Unfilt_Angular_Rate;
extern Vector	IMU_Unfilt_Angular_Rate_Old;
extern Vector	IMU_Angular_Accel;
extern matrix	IMU_Xfm;
extern Scalar	IMU_Angular_BW;
double		IMU_Temp_Data[6];
extern int	IMU_Ang_Sample_Mode;
extern int	IMU_Total_Ang_Samples;
extern Vector	IMU_Total_Ang_Sum;
/****************************************
*					*
*	IMU message handler		*
*	prototypes.			*
*					*
*****************************************/
MsgHandler 	IMU_Ch0Data( MsgPtr msg);
MsgHandler 	IMU_Ch1Data( MsgPtr msg);
MsgHandler 	IMU_ShutDown( MsgPtr msg);
void		IMU_Transform(void);
/****************************************
*					*
*	Accelerometer x-axis		*
*					*
*****************************************/
MsgHandler IMU_Ch0Data( MsgPtr msg)
{
    IMU_Temp_Data[0]= -(((((int)msg->data[1]<<4) | msg->data[3]>>4)  *0.00488 - 10.0)*0.28   );
    IMU_Temp_Data[1]=  (((((int)msg->data[2]<<4) | msg->data[3]&0x0f)*0.00488 - 10.0)*0.28   );
    IMU_Temp_Data[2]=  (((((int)msg->data[4]<<4) | msg->data[5]>>4)  *0.00488 - 10.0)*0.28   );
/*printf("hello\n\r");	*/	
    return OK;
}
/****************************************
*					*
*	Accelerometer y-axis		*
*					*
*****************************************/
MsgHandler IMU_Ch1Data( MsgPtr msg)
{
struct	timespec *tp;
Vector	unfilt_dwd;
Scalar	GAMMA, DT;
static	int running = 0;
long	time, oldtime, dt;	
    IMU_Temp_Data[4]=  ((((((int)msg->data[1]<<4) | msg->data[3]>>4)  *0.00488 - 10.0))*0.17453293   );
    IMU_Temp_Data[5]= -((((((int)msg->data[2]<<4) | msg->data[3]&0x0f)*0.00488 - 10.0))*0.17453293   );
    IMU_Temp_Data[3]= -((((((int)msg->data[4]<<4) | msg->data[5]>>4)  *0.00488 - 10.0))*0.17453293   );
/*
    IMU_Unfilt_Angular_Rate = MultMV(IMU_Xfm, VectorInit(3, IMU_Temp_Data[3], IMU_Temp_Data[4], IMU_Temp_Data[5]));
    clock_gettime(CLOCK_REALTIME, tp);
    time = tp->tv_nsec;
    if(running == 0){
	oldtime = tp->tv_nsec;
	running = 1;
    }else{
	dt = time - oldtime;
	oldtime = time;
	if(dt<0)dt+=1000000000;
	if(DT>0.){
	    DT = ((float)dt)*.000000001;
	    IMU_Angular_BW = 1./DT;
	}
	GAMMA = .8;
	unfilt_dwd = ScalarMultSV(1./DT, AddVV(IMU_Unfilt_Angular_Rate, ScalarMultSV(-1., IMU_Unfilt_Angular_Rate_Old)));
	IMU_Angular_Accel = AddVV( ScalarMultSV(GAMMA, IMU_Angular_Accel), ScalarMultSV((1.-GAMMA), unfilt_dwd));
	IMU_Unfilt_Angular_Rate_Old = IMU_Unfilt_Angular_Rate;
    }
*/
    IMU_Transform();
    return OK;
}
void	IMU_Transform(void)
{
static	int counter = 0;
static	int start = 0;
static	Vector	localIMU;
Vector	dat;
Vector	angRate;
    if(start == 0){
	start = 1;
	localIMU = VectorInit(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
    }  
    localIMU = AddVV(localIMU, VectorInit(6, IMU_Temp_Data[0], IMU_Temp_Data[1], 
					     IMU_Temp_Data[2], IMU_Temp_Data[3], 
					     IMU_Temp_Data[4], IMU_Temp_Data[5]));
    counter++;
    if (counter==10){
	dat = ScalarMultSV(1./((float)10),localIMU);
	IMU_Acceleration = MultMV(IMU_Xfm, VectorInit(3, V(dat, 1), V(dat, 2), V(dat, 3)));
	angRate = MultMV(IMU_Xfm, VectorInit(3, V(dat, 4), V(dat, 5), V(dat, 6)));
	
	if(IMU_Ang_Sample_Mode){
	    IMU_Total_Ang_Samples++;
	    IMU_Total_Ang_Sum = AddVV(IMU_Total_Ang_Sum, angRate);
	}
	IMU_Angular_Rate = AddVV(angRate, ScalarMultSV(-1.0,IMU_Angular_Rate_Bias));
	IMU_Data = VectorInit(6, V(IMU_Acceleration,1), V(IMU_Acceleration,2), V(IMU_Acceleration,3),
			  V(IMU_Angular_Rate,1), V(IMU_Angular_Rate,2), V(IMU_Angular_Rate,3));
    
	/*PrintV(IMU_Data);printf("\r");*/
	localIMU = VectorInit(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
	counter = 0;	    
    }
}
/* IMU_ShutDown
 *
 */
MsgHandler IMU_ShutDown( MsgPtr msg)
{
/* function which kills all hardware outputs when fatal errors in
	communication occur
*/
	return( OK);
}
/* End Of File */

End IMU Software
Begin ESU Message Handler Software

/********************************************************************************
*										
*	imu_MiMiCS_handlers.c
*
*	This file contains the message handlers for the 
*	Ranger NBV Inertial Measurement Unit.
*
*	Written by Joe Graves and JeanMarc Henriette May 1995
*										
*********************************************************************************/
#include <vxWorks.h>
#include <pivecs.h>
#include <math.h>
#include <stdio.h>
#include <vector_jdg.h>
#include "esu_mm.h"
/****************************************
*					*
*	Globals				*
*					*
*****************************************/
extern	ChanPtr ESU_MMChannel;
extern	Vector	ESU_Magnetometer_Data;
extern	Vector	ESU_Magnetometer_Bias;
extern	int	ESU_Mag_Bias_Est_Run;
/*extern	Scalar	ESU_Depth;*/
/****************************************
*					*
*	IMU message handler		*
*	prototypes.			*
*					*
*****************************************/
MsgHandler esuTestMsg( MsgPtr msg);
MsgHandler esuShutDown( MsgPtr msg);
MsgHandler esuRXdata1( MsgPtr msg);
MsgHandler esuRXdata2( MsgPtr msg);
MsgHandler esuShowDump( MsgPtr msg);
static int ESU_AD[8];
/****************************************
*					*
*	Read Magnetometer Data		*
*					*
*****************************************/
MsgHandler esuRXdata1( MsgPtr msg)
{
Vector	tmp,bias;
static	int start = 0;
static	int count = 0;
static	int countPr = 0;
static	int countPr2 = 0;
static	Vector	avg, ESU_Magnetometer_Bias;
Vector	Bsen, Badj;
Scalar	BsenMag;
Vector	Numerator;
Scalar	Denominator;
int	num;
    num = 10;
    ESU_AD[3]= ((int)msg->data[1]<<4) | msg->data[3]>>4 ;
    ESU_AD[1]= ((int)msg->data[2]<<4) | msg->data[3]&0x0f ;   
    ESU_AD[2]= ((int)msg->data[4]<<4) | msg->data[6]>>4 ;
    ESU_AD[0]= ((int)msg->data[5]<<4) | msg->data[6]&0x0f ;
    /*printf("%d %d %d %d %d %d \n\r",(int)msg->data[1], (int)msg->data[2], (int)msg->data[3], (int)msg->data[4], (int)msg->data[5], (int)msg->data[6]); */
    /* (tmp) This is the magnetic field (Gauss) indicated by the magnetometer
	    and the a/d board */
    tmp = ScalarMultSV(1.0/2048.0,
	VectorInit(3,(Scalar)(-(ESU_AD[0]-2047)),	/* the two minus signs flip the x and y axes into vehicle frame */
		     (Scalar)(-(ESU_AD[1]-2047)),
		     (Scalar)( (ESU_AD[2]-2047))));
/*  PrintV(tmp);printf("\r"); */
/*  ESU_Depth = (Scalar)ESU_AD[3]; */
    if (start==0){
	ESU_Magnetometer_Bias = VectorInit(3, .25, 0.02, 0.);
	avg = VectorInit(3, 0., 0., 0.);
	start = 1;
    }
    avg = AddVV(avg, tmp);
    count++;
    if(count == num){
	Bsen = ScalarMultSV(1./((Scalar)(num)), avg);
	avg = VectorInit(3, 0., 0., 0.);
/*
	BsenMag = MagnitudeV(Bsen);
	ESU_Magnetometer_Bias = AddVV(ESU_Magnetometer_Bias, AddVV(ScalarMultSV(BsenMag, UnitV(Bsen)), ScalarMultSV(-1, Bsen)));
*/
/******* Bias Estimator ********/ 
	if(ESU_Mag_Bias_Est_Run == TRUE){
	    Numerator = AddVV(Bsen, ScalarMultSV(-1., ESU_Magnetometer_Bias));
	    Denominator = MagnitudeV(AddVV(Bsen, ScalarMultSV(-1., ESU_Magnetometer_Bias)));
	    /*ESU_Magnetometer_Bias = AddVV(Bsen, ScalarMultSV(-1., ScalarMultSV(.48/Denominator, Numerator)));*/
	    ESU_Magnetometer_Bias = AddVV(Bsen, ScalarMultSV(-1., ScalarMultSV(.475/Denominator, Numerator)));
	    if(countPr++ > 20){
		printf("Magnetometer Bias");PrintV(ESU_Magnetometer_Bias);printf("\r");
		countPr = 0;	    
	    }
	}	
	ESU_Magnetometer_Data = AddVV(Bsen, ScalarMultSV(-1., ESU_Magnetometer_Bias));
	if(countPr2++ > 200){
	    printf("Magnetometer Data Magnitude");PrintS(MagnitudeV(ESU_Magnetometer_Data));printf("\n\r");
	    countPr2 = 0;	    
	}
	count = 0;
    }
    return OK;
}
MsgHandler esuRXdata2( MsgPtr msg)
{
	      
	ESU_AD[4]= ((int)msg->data[1]<<4) | msg->data[3]>>4 ;
	ESU_AD[5]= ((int)msg->data[2]<<4) | msg->data[3]&0x0f ;
	ESU_AD[6]= ((int)msg->data[4]<<4) | msg->data[6]>>4 ;
	ESU_AD[7]= ((int)msg->data[5]<<4) | msg->data[6]&0x0f ;
	return OK;
}
/* esuShutDown
 *
 *      This function is not the way to quit the program, or power down the control system.
 *  This is the first function which you should create and it is perhaps
 *      the most important one of the whole program.  This function is called
 *      whenever PiVeCS has lost comunications with the remote terminal.  If this
 *      implementation is being run on a vehicle (MPOD for example) and that vehicle
 *      loses communications with the pilot or 3ADPS, then this function must immediately
 *      shut off all of the motors, and stop doing anything which could cause the vehicle
 *      to move or act unexpectedly.
 *
 *      Arguments:
 *              msg             a pointer to a message structure
 *
 *      Returns:
 *              Byte    boolean response to indicate whether the function was carried out
 *                              successfully or not.
 */
MsgHandler esuShutDown( MsgPtr msg)
{
/* function which kills all hardware outputs when fatal errors in
	communication occur
*/
	return( OK);
}
/* added back by Phil to show data dumps from mmconfig messages */
MsgHandler esuShowDump( MsgPtr msg)
{
int      addr;
static int      line = 5;
	addr = (msg->data[1] << 8) + msg->data[2];
	printf( "%2d  %4X  %2X %2X %2X %2X\n",msg->data[0], addr, msg->data[3],
			msg->data[4], msg->data[5], msg->data[6]);
	if( line > 20)
		line = 0;
	return(OK);
}
/* End Of File */

End ESU Message Handler Software