APPENDIX F: OPERATOR INTERFACE SOFTWARE


#include <stdio.h>		/* printf */
#include <stdlib.h>		/* getenv */
#include <math.h>
#include "forms.h"
#include "vector.h"
/*#include "graph.h"*/
#include "egi_forms.h"
extern void		egiInitNDDS( void);
extern void		egiCheckNDDS(void);
extern void		Init3D(void);
extern void		Update3D(void);
extern void		Close3D(void);
/*extern void		egiEndNDDS(void);*/
Vector		Magnetometer;
Vector		Accelerometer;
Vector		AngularRate;
Vector		Desired_Quaternion;
Vector		Estimated_Quaternion;
Vector		Quaternion_Error;
Vector		Omega_Error;
Vector		Tau;
int		DONE;
Scalar		Kp;
Scalar		Kd;
void	main(int argc, char **argv)
{
int	i;
Scalar	time;
Vector	vv;
    Kp=0.0;
    Kd=0.0;
    Init3D();
    gui_init(argc,argv);
    egiInitNDDS();
    DONE = False;
    while(DONE==False) {
	egiCheckNDDS();
	check_forms();
	Update3D();
    }
    Close3D();
    /*egiEndNDDS();*/
}

Begin Communications Interface Software


/*****************************************************************************
*
*	NDDS interface for the estimator graphics module
*
for sympol table update:
activate script /vc_proj/cs/nddsCreate/makeRangerSymbol
recompile control station
copy sTCB_ranger.c to this directory
******************************************************************************/
#include <stdio.h>		/* printf */
#include <stdlib.h>		/* getenv */
#include <math.h>
#include "vector.h"
#include "sTcons_ranger.h"
#include "sTprod_ranger.h"
#include "sTvar_ranger.h"
#include "sTndds_ranger.h"
#include "wm2.0/wm.h"
void		egiProportionalGainSend(Vector v);
void		egiDerivativeGainSend(Vector v);
void		egiControlSend(int i);
void		egiTrajectorySend(int i);
void		egiRollStepSend(int i);
void		egiPitchStepSend(int i);
void		egiYawStepSend(int i);
void		egiThrScaleSend(Scalar s);
void		egiAngFilterSend(Scalar s);
void		egiAdaptSend(Scalar s);
void		egiTrjSend(int i, Scalar s);
Vector				vec;
extern	Vector			estState;
extern	Vector			desState;
extern	Vector			Magnetometer;
extern	Vector			Accelerometer;
extern	Vector			AngularRate;
extern	Vector			Desired_Quaternion;
extern	Vector			Estimated_Quaternion;
extern	Vector			Quaternion_Error;
extern	Vector			Omega_Error;
extern	Vector			Tau;
extern	int			DONE;
extern	Scalar			Kp;
extern	Scalar			Kd;
Scalar			Adapt;
extern	WMbuf	buf[10];
/******************************************************************************
* Initialize NDDS
******************************************************************************/
void	egiInitNDDS()
{
    allocateNddsVariableMemory();
    printf("Variable memory allocated\n");
    
    setUpCommandProducers(0);
    printf("Command producers set up\n");
    
    setUpTelemetryConsumers(0);
    printf("Telemetry producers set up\n");
}
void	egiCheckNDDS()
{
    TelemetryPollConsumers();
    /*NddsConsumerPoll(EstAC);
     *
     */
}
void egiAdaptSend(Scalar s)
{	
    printf("adapt %f\n\r", (float)s);
    ST_Est_Adaptation_Rate->value = (float)s;
    NddsProducerSample(Est1P);
}
void egiControlSend(int i)
{	
    printf("Control %d\n\r", (int)i);
    ST_Est_Control->value = i;
    NddsProducerSample(Est2P);
}
void egiDerivativeGainSend(Vector v)
{
int i;	
    PrintV(v);printf("D Gain\n\r");
    for(i=1;i<=Dim(v);i++){
	ST_Est_Derivative_Gain->array.array_val[i-1] = V(v,i);
    }
    NddsProducerSample(Est3P);
}
void egiPitchStepSend(int i)
{	
    printf("Pitch %d\n\r", (int)i);
    ST_Est_Pitch_Step->value = i;
    NddsProducerSample(Est4P);
}
void egiProportionalGainSend(Vector v)
{
int i;	
    PrintV(v);printf("P Gain\n\r");
    for(i=1;i<=Dim(v);i++){
	ST_Est_Proportional_Gain->array.array_val[i-1] = V(v,i);
    }
    NddsProducerSample(Est5P);
}
void egiRollStepSend(int i)
{	
    printf("Roll %d\n\r", (int)i);
    ST_Est_Roll_Step->value = i;
    NddsProducerSample(Est6P);
}
void egiThrScaleSend(Scalar s)
{	
    printf("Scale %f\n\r", (float)s);
    ST_Est_Thruster_Scale->value = (float)s;
    NddsProducerSample(Est7P);
}
void egiAngFilterSend(Scalar s)
{	
    printf("Filter %f\n\r", (float)s);
    ST_Est_Trajectory_Control->i = 20;
    ST_Est_Trajectory_Control->d = s;
    NddsProducerSample(Est9P);
}
void egiTrajectorySend(int i)
{	
    printf("Trajectory %d\n\r", (int)i);
    ST_Est_Trajectory->value = i;
    NddsProducerSample(Est8P);
}
void egiTrjSend(int a, Scalar s)
{	
    printf("Trajectory values %d, %f\n\r", (int)a, (float)s);
    ST_Est_Trajectory_Control->i = a;
    ST_Est_Trajectory_Control->d = s;
    NddsProducerSample(Est9P);
}
void egiYawStepSend(int i)
{	
    printf("Yaw %d\n\r", (int)i);
    ST_Est_Yaw_Step->value = i;
    NddsProducerSample(Est10P);
}
void	ST_Est_Accelerometer_FN(NDDSUpdateInfo update, darray data){
int	i;
Vector	v;
	Dim(v) = (unsigned int)(data->array.array_len);
	for(i=1;i<=Dim(v);i++){
		V(v,i)=data->array.array_val[i-1];
		/*printf("%f ", (float)data->array.array_val[i-1]);*/
	}
	/*printf(" accelerometer\n\r");*/
	acc_update(v);
	Accelerometer=v;
	return;
}
void	ST_Est_Desired_Quaternion_FN(NDDSUpdateInfo update, darray data){
int	i;
Vector	v;
	Dim(v) = (unsigned int)(data->array.array_len);
	for(i=1;i<=Dim(v);i++){
		V(v,i)=data->array.array_val[i-1];
		/*printf("%f ", (float)data->array.array_val[i-1]);*/
	}
	/*printf(" Desired Quaternion\n\r");*/
	Desired_Quaternion = v;
	V(desState, 7)=V(v, 1);	
	V(desState, 8)=V(v, 2);	
	V(desState, 9)=V(v, 3);	
	V(desState,10)=V(v, 4);
	return;
}
void	ST_Est_Error_Quaternion_FN(NDDSUpdateInfo update, darray data){
int	i;
Vector	v;
	Dim(v) = (unsigned int)(data->array.array_len);
	for(i=1;i<=Dim(v);i++){
		V(v,i)=data->array.array_val[i-1];
		/*printf("%f ", (float)data->array.array_val[i-1]);*/
	}
	/*printf(" error quaternion\n\r");*/
	Quaternion_Error = v;
	quat_update();
	return;
}
void	ST_Est_Estimated_Quaternion_FN(NDDSUpdateInfo update, darray data){
int	i;
Vector	v;
	Dim(v) = (unsigned int)(data->array.array_len);
	for(i=1;i<=Dim(v);i++){
		V(v,i)=data->array.array_val[i-1];
		/*printf("%f ", (float)data->array.array_val[i-1]);*/
	}
	/*printf(" estimated quaternion\n\r");*/
	Estimated_Quaternion = v;
	V(estState, 7)=V(v, 1);	
	V(estState, 8)=V(v, 2);	
	V(estState, 9)=V(v, 3);	
	V(estState,10)=V(v, 4);
	
	return;
}
void	ST_Est_Magnetometer_FN(NDDSUpdateInfo update, darray data){
int	i;
Vector	v;
	Dim(v) = (unsigned int)(data->array.array_len);
	for(i=1;i<=Dim(v);i++){
		V(v,i)=data->array.array_val[i-1];
		/*printf("%f ", (float)data->array.array_val[i-1]);*/
	}
	/*printf(" magnetometer\n\r");*/
	mag_update(v);
	Magnetometer=v;
	return;
}
void	ST_Est_Omega_Error_FN(NDDSUpdateInfo update, darray data){
int	i;
Vector	v;
	Dim(v) = (unsigned int)(data->array.array_len);
	for(i=1;i<=Dim(v);i++){
		V(v,i)=data->array.array_val[i-1];
		/*printf("%f ", (float)data->array.array_val[i-1]);*/
	}
	/*printf(" angular rate error\n\r");*/
	wmAddToBuffer(buf[0], V(v, 1));
	wmAddToBuffer(buf[1], V(v, 2));
	wmAddToBuffer(buf[2], V(v, 3));
	Omega_Error = v;
	
	v = Quaternion_Error;
	wmAddToBuffer(buf[6], sqrt(V(v, 1)*V(v, 1)+V(v, 2)*V(v, 2)+V(v, 3)*V(v, 3)));
	return;
}
void	ST_Est_Rate_FN(NDDSUpdateInfo update, darray data){
int	i;
Vector	v;
	Dim(v) = (unsigned int)(data->array.array_len);
	for(i=1;i<=Dim(v);i++){
		V(v,i)=data->array.array_val[i-1];
		/*printf("%f ", (float)data->array.array_val[i-1]);*/
	}
	/*printf(" angular rate\n\r");*/
	ang_update(v);
	AngularRate = v;
	return;
}
void	ST_Est_Tau_FN(NDDSUpdateInfo update, darray data){
int	i;
Vector	v;
	Dim(v) = (unsigned int)(data->array.array_len);
	for(i=1;i<=Dim(v);i++){
		V(v,i)=data->array.array_val[i-1];
		/*printf("%f ", (float)data->array.array_val[i-1]);*/
	}
	/*printf(" torque\n\r");*/
	wmAddToBuffer(buf[3], V(v, 1));
	wmAddToBuffer(buf[4], V(v, 2));
	wmAddToBuffer(buf[5], V(v, 3));
	Tau = v;
	tau_update();
	return;
}