//  ----------------------------------------------------------------------------
//  Includes
//  ----------------------------------------------------------------------------

#include "_options.h"
#include "ArduWin.h"
#include "gimbalDef.h"
#include "filter.h"
#include "SVNVersion.h"
#include <math.h>
#include "math3D.h"

//  ---------------------
//  Functions
//  ---------------------

void gimbalControl();
void monitor();
void angles();
void transform1();
void sendVNSIH(float heading);
void commHit(comStats_t *s);
void commCRC(comStats_t *s);
int readVN200();
unsigned short calculateCRC(unsigned char data[], unsigned int length);
void sendLog();
void toLog(uint32_t type);
void toLog(const char *s, uint32_t type = LOG_GENERIC);
int utilSend(unsigned char *msg, int len, int addchecksum = 1);
void readOuterAnalog();
void logBuild();
void innerPwmOut(int axis, float drive, int standby);
void redControls();
void boresight();
void boresightF();
int encoderValid(int axis);
void parseVN200(navState_t *_navState);
float analogFilter(int axis,float in);
float losFilter(int axis,float in);
float geoFilter(int axis,float in);
float innerTachFilter(int axis,float in);
void utilCommsRx();
void passive();
void passiveRate();
void metersToLatlon(double x,double y, double *latP, double *lonP, double lat0, double lon0);
int latLonToMeters(double lat, double lon, double *xP, double *yP, double lat0, double lon0);
int latLonToMeters(double lla[3], double *xP, double *yP, double lat0, double lon0);
int latLonToMeters(double lla[3], double *xP, double *yP, double lla0[3]);
int latLonToNeu(double lla[3], double (*neu)[3], double lla0[3]);

void boresight();
void boresightF();
void boresightD();
void insertChecksum(unsigned char *msg, int len);
void sendLensCommand();
void proc2000();
void killPid();
float outerPwmScale(float x);

//  ---------------------
//  Strings
//  ---------------------

const char* logStrings[NUM_LOG_TYPES] = {LOG_STRINGS};
const char* typeStrings[NUM_TYPE] = {TYPE_STRINGS};

//  ---------------------
//  Serial
//  ---------------------

	#if defined(_OUTER)
		#define SerialGim	Serial5
		#define SerialUtil SerialUSB
	#else
		#define SerialVN	Serial1
		#define SerialCam	Serial2
		#define SerialCons	Serial3
		#define SerialGyro	Serial4
		#define SerialGim	Serial5
		#define SerialLens	Serial6
		#define SerialUtil SerialUSB
	#endif

	#define txUtilEmpty txUEmpty
	#define serialUtilWrite serialUWrite

//  ---------------------
//  Analog
//  ---------------------

	#if defined(_OUTER)
		#define A_YAW	A15
		#define A_PIT	A16
	#else
		#define A_YAW	A14
		#define A_PIT	A15
		#define A_ROL	A16
	#endif

//  ---------------------
//  Constants
//  ---------------------

enum{
	SEQ_INIT,
	SEQ_VSW1,
	SEQ_VSW2,
	SEQ_VSW3,
	SEQ_VSW4,
	SEQ_VSW5,
	SEQ_VSW6,
	SEQ_VSW7,
	SEQ_DOM1,
	SEQ_DOM2,
	SEQ_DOM3,
	SEQ_INDEXED1,
	SEQ_INDEXED2,
	SEQ_OPERATIONAL,
	SEQ_FAIL1,
	SEQ_FAIL2
};

//  ---------------------
//  Constants
//  ---------------------

enum{
	geo_None,
	geo_VectorNav,
	geo_OdinSim
};

//  ---------------------
//  Constants
//  ---------------------

#ifdef _LENS
	#define T3PERIOD 2
#else
	#define T3PERIOD 5
#endif

	const int ticksPerSecond = 1000000/T3PERIOD;
	const int ticksPerMilliSec = 1000/T3PERIOD;
	const float ticksPerSecondF = 1000000.0f/T3PERIOD;
	const float ticksPerMilliSecF = 1000.0f/T3PERIOD;

//  ---------------------
//  Constants
//  ---------------------

	const float kfilt = 0.0025f;				// Joystick smoothing
	const int kpwm = 127;						// PWM scaling
	const int kpwmL = 255;						// PWM scaling Lens
	const int kaOfs = 2048;						// Analog input offset
	const float kaScale = 1.0f/2048.0f;			// Analog input scale
	const int jitCount = 1024;					// Jitter buffer size
	const float kTach = 300.f;
	const float kaFilt = 0.2f;

	const float deadband = 0.3f;				// Joystick deadband

#if defined(_004)
	#if defined(_OUTER)
		const float kAngleGainY = -360/(0.198f + 0.203f);
		const float kAngleGainP =  90/0.506f;
		const float kAngleGainR =   0;
		const float kAngleOffsY =   0.0035f;
		const float kAngleOffsP =   0.285f;
		const float kAngleOffsR =   0;
	#else
		const float kAngleGainY = -10.0f/0.16f;
		const float kAngleGainP =  10.0f/0.16f;
		const float kAngleGainR =  -90.0f/(0.833f+0.856f);
		const float kAngleOffsY = (-0.122f+0.054f)/2;
		const float kAngleOffsP = ( 0.082f-0.070f)/2;
		const float kAngleOffsR = (-0.833f+0.856f)/2;
	#endif
	const float kSpeedY = 0.05f;
	const float kSpeedP = kSpeedY;
	const float kSpeedR = kSpeedY;
	const float kGyroZeroY = -0.090f;
	const float kGyroZeroP = -0.010f;
	const float kGyroZeroR = -0.040f;
	const float kDomeEnc = 360.f/(512.f*4*4);
	const float kVswEnc = -360.f/(512.f*4*3);
	const float kDomeInx = 9.7f - 1.5f;
	const float kVswInx = 3.0f;
	const float kDomeOfs = -kDomeInx + 0.158f;
	const float kVswOfs = -kVswInx - 0.281f;
	const float kBoresightY =  0.00f;
	const float kBoresightP =  0.00f;
	const float kBoresightR =  0.00f;
#endif

//	---------------------
//  aiFilter_t
//	---------------------

typedef struct
{
	float a0;
	float a1;
	float a2;
	float a3;
	float a;
	float f1;
	float f2;
	int aBuf[8];
	int aInx;
}  aiFilter_t;

//	---------------------
//  gyroBuffer_t
//	---------------------

typedef struct {
	uint64_t timestamp;
	float minval;
	float maxval;
	float samples[NSAMP];
} gyroBuffer_t;  

//  ---------------------
//  PID
//  ---------------------

#if 0

	const int numInitFiltAxes = 3;
	const int numInitFilt = 3;

	const float kpY =   3;		const float kiY = 0.005f;		const float kdY = 0.03f;
	const float kpP =   3;		const float kiP = 0.010f;		const float kdP = 0.03f;
	const float kpR =   2;		const float kiR = 0.020f;		const float kdR = 0.00f;
	const float kiL = 0.23f;

	pidPar_t pidPar[NiAXES]={
	//		 kP{rate,    geo,    pos } kI{rate,   geo,   pos}  kD{rate,   geo,    pos } kILim{rate, geo, pos }       kT  kC
		{	{    kpY,    kpY,      2 },{   kiY,   kiY,   kiY },{   kdY,   kdY,  0.02f },{      kiL, kiL, kiL },	0.100f,  1 },		// Yaw
		{	{    kpP,    kpP,      2 },{   kiP,   kiP,   kiP },{   kdP,   kdP,  0.02f },{      kiL, kiL, kiL },	0.250f,  1 },		// Pitch
		{	{    kpR,    kpR,    kpR },{   kiR,   kiR,   kiR },{   kdR,   kdR,    kdR },{      kiL, kiL, kiL },	0.025f,  0 }		// Roll
	};

	filterPar_t filtPi[numInitFiltAxes][numInitFilt]={
		{	
			{FILTER_LAGLEAD,  30,  10},		// Yaw
			{FILTER_NOTCH,   170,   4},
			{FILTER_NONE,    205,  10},
		},{	
			{FILTER_LAGLEAD,  30,  10},		// Pitch
			{FILTER_NOTCH,   190,   5},
			{FILTER_NONE,    200,  10},
		},{	
			{FILTER_LAGLEAD,  10,  40},		// Roll
			{FILTER_NONE,    100,  10},
			{FILTER_NONE,    100,  10},
		}
	};

#elif defined(_001)

	const int numInitFiltAxes = 3;
	const int numInitFilt = 3;
	const float kpY = 200;		const float kiY = 1.00f;		const float kdY = 1.00f;
	const float kpP =  45;		const float kiP = 0.70f;		const float kdP = 0.30f;
	const float kpR =   2;		const float kiR = 0.02f;		const float kdR = 0.00f;
	const float kiL = 0.23f;

	pidPar_t pidPar[NiAXES]={
	//		 kP{rate,    geo,    pos } kI{rate,   geo,   pos}  kD{rate,  geo,    pos } kILim{rate, geo, pos }       kT  kC
		{	{    kpY,    kpY,    kpY },{   kiY,   kiY,   kiY },{   kdY,  kdY,     kdY },{      kiL, kiL, kiL },	0.075f,  1 },		// Yaw
		{	{    kpP,    kpP,    kpP },{   kiP,   kiP,   kiP },{   kdP,  kdP,     kdP },{      kiL, kiL, kiL },	0.200f,  1 },		// Pitch
		{	{    kpR,    kpR,    kpR },{   kiR,   kiR,   kiR },{   kdR,  kdR,     kdR },{      kiL, kiL, kiL },	0.025f,  0 }		// Roll
	};

	filterPar_t filtPi[numInitFiltAxes][numInitFilt]={
		{	
			{FILTER_LAGLEAD,   2,  60},		// Yaw
			{FILTER_NOTCH,   190,   5},
			{FILTER_NOTCH,   205,  10},
		},{	
			{FILTER_LAGLEAD,  16,  10},		// Pitch
			{FILTER_NOTCH,   185,   5},
			{FILTER_NONE,    200,  10},
		},{	
			{FILTER_LAGLEAD,  10,  40},		// Roll
			{FILTER_NONE,    100,  10},
			{FILTER_NONE,    100,  10},
		}
	};
#elif defined(_004)

	const int numInitFiltAxes = 3;
	const int numInitFilt = 4;
	const float kpY = 20;		const float kiY = 0.30f;		const float kdY = 0.25f;
	const float kpP = 12;		const float kiP = 0.20f;		const float kdP = 0.10f;
	const float kpR = 15;		const float kiR = 0.02f;		const float kdR = 2.00f;
	const float kiL = 0.23f;

	pidPar_t pidPar[NiAXES]={
	//		 kP{rate,    geo,    pos } kI{rate,   geo,    pos}  kD{rate,  geo,    pos } kILim{rate, geo, pos }       kT  kC
		{	{    kpY,    kpY,    kpY },{   kiY,   kiY,  0.10f },{   kdY,  kdY,     kdY },{      kiL, kiL, kiL },	0.200f,  1 },		// Yaw
		{	{    kpP,    kpP,    kpP },{   kiP,   kiP,  0.15f },{   kdP,  kdP,     kdP },{      kiL, kiL, kiL },	0.200f,  1 },		// Pitch
		{	{    kpR,    kpR,    kpR },{   kiR,   kiR,    kiR },{   kdR,  kdR,     kdR },{      kiL, kiL, kiL },	0.025f,  0 }		// Roll
	};

	filterPar_t filtPi[numInitFiltAxes][numInitFilt]={
		{	
			{FILTER_LAGLEAD,  30,  25},		// Yaw
			{FILTER_NOTCH,   110,  10},
			{FILTER_NOTCH,   200,   4},
			{FILTER_NOTCH,   310,   5},
		},{	
			{FILTER_LAGLEAD,  30,  25},		// Pitch
			{FILTER_NOTCH,   220,  10},
			{FILTER_NOTCH,   310,   5},
			{FILTER_NONE,    100,  10},
		},{	
			{FILTER_LAGLEAD,   4, 100},		// Roll
			{FILTER_NONE,    100,  10},
			{FILTER_NONE,    100,  10},
			{FILTER_NONE,    100,  10},
		}
	};
#endif

//  ---------------------
//  Outer PID
//  ---------------------

#if defined(_004)
		outerPar_t outerPar[NoAXES]={
	//		 kP	  kFF     kTach      kI  kILim
		{  5.0f,	1,	 0.006f, 0.010f,     1  },		// Dome
		{  5.0f,	1,	 0.006f, 0.010f,     1  }		// VSW
	};
#endif

//  ---------------------
//  Position Limits
//  ---------------------

#if defined(_004)

	int hasOuterStops[NoAXES] = {1,1};
	float outerStops[NoAXES][2] = {
		{-630, 630},			// 650
		{-120,  20}
	};
	float stowPos[NoAXES] = {180,0};

#endif

//  ---------------------
//  Rate Limits
//  ---------------------

#if defined(_004)
	const float maxrate[NiAXES] =	{	100,	 90,	 80};
	const float maxaccO[NiAXES] =	{   500,    500,    500};		// max acc for Outer drives
	const float maxacc2[NiAXES] =	{	500,	500,	500};		// max acc for xxx mode
	const float maxacc3[NiAXES] =	{	500,	500,	500};		// max acc for stow mode
	float kOverspeed = 120;
	float kStandbyVswDamp = 2;
#endif

/*
	const float maxrate[NiAXES] =	{	190,	190,	 80};
	const float maxacc1[NiAXES] =	{   900,    900,    550};		// max acc for Outer drives
	const float maxacc2[NiAXES] =	{   900,    900,	500};		// max acc for all modes
	const float maxacc3[NiAXES] =	{	200,	200,	200};		// max acc for stow mode
	#define kOverspeed 260
	float kStandbyVswDamp = 0.2f;



*/
//  ---------------------
//  Vars
//  ---------------------

	float innerTach[3];
	float losTach[3];
	float navRate[3];
	float xform[3] = {1,1,1};
	float frameGeoError[2];
	float frameGeoRate[2];
	float framePixScore;

//  ---------------------
//  Vars
//  ---------------------

	controls_t controls;				// Gimbal Controls
	state_t state;						// Gimbal state
	geoMisc_t geoMisc;
	geoMisc_t geoMisc2;
	steering_t steering;
	int odinSim = 0;
	int odinConfigEcho = 0;
	int odinLensStatusReq = 0;
	int odinAckTargetReq = 0;
	int odinAckLensCalReq = 0;
	int odinConnected = 0;
	int64_t fireStartTime;
	int64_t midExpTime;
	int asyncFireCameraMode = 0;
	int newGpsSim = 0;
	int newGpsSimLL = 0;
	int sendHsLogs = 0;
	int newNavCs = 0;
	int newGeoTargetB = 0;
	lensCommand_t lensCommand[NLENS];
	char build[16];

//  ---------------------
//  vars
//  ---------------------

	int geoFocus;
	int geoFocusPos0 = 10000;
	int initFocusReq;
	int sendIndexEnableReq;
	int vswEncoderZero = 0;
	volatile int32_t bump[NoAXES3] = {0,0,0};
	volatile int32_t ecount[NoAXES3] = {0,0,0};
	volatile int32_t indexed[NoAXES3] = {0,0,0};
	float maxOuterDriveP[NoAXES3];
	float maxOuterDriveN[NoAXES3];
	int zerocount[NoAXES3];
	int tcount[NoAXES3];
	float tTachScale[NoAXES3] = {
		ticksPerSecondF*kDomeEnc,
		ticksPerSecondF*kVswEnc,
		ticksPerSecondF*kDomeEnc
	};
	float tTach[NoAXES3];

//  ---------------------
//  vars
//  ---------------------

#if defined(_004)
	int enableIndex[NoAXES3] = {1,1,0};
#endif

//	----------------------------------------------------------------------------
//  doPidOuter
//	----------------------------------------------------------------------------

	const unsigned int kfadein = 2*kFreq;
	unsigned int fadein = kfadein;
	float outerErrorInt[NoAXES];
	float state_rateCommandOuter[3];

//  ---------------------
//  msgSource
//  ---------------------

	enum{
		srcNone = 0,
		srcUtil,
		srcMain,
		srcCons,
		srcLens,
	};

	int msgSource = srcNone;
	int connectUtil;
	int connectGim;
	int connectCons;

//  ---------------------
//  Vars
//  ---------------------

	int plotAxis;						// Safe version of controls.plotAxis
	int sampleIndex;					// gyroBuffer index
	volatile int sampleHead;			// gyroBuffer head count
	int sampleTail;						// gyroBuffer tail count
	int16_t messagecount;				// TX message count
	filter filt;						// Filters class

	double xi2c;
	double yi2c;
	double zi2c;

	int gyroOK = 0;
	int step = 0;
	aiFilter_t angleFilt[3];
	aiFilter_t tachFilt[3];
	float stime[3];
	int smooth = 0;
	float smoothAcc[NiAXES3];
	float smoothRate[NiAXES3];
	float dynRate[NiAXES3];
	double neRange;

#ifdef _PHASE1
	const unsigned int kTrigPulseWidth = 5*ticksPerMilliSec; 
	const unsigned int kMinFrameTime = 130*ticksPerMilliSec; 
#else
	const unsigned int kTrigPulseWidth = 5*ticksPerMilliSec; 
	const unsigned int kMinFrameTime = 130*ticksPerMilliSec; 
#endif
	const unsigned int trigK2 = max(kTrigPulseWidth+2,kMinFrameTime);
	const unsigned int trigK1 = trigK2 - kTrigPulseWidth; 

	volatile int64_t ticks = 0;
	volatile int tick1 = 0;
	volatile int txtimer = 0;
	volatile uint32_t trigtimer = 0;
	volatile int expcount = 0;
	int64_t ticksCommon = 0;

	int vn200Connected;	// ToDo

//	-------------------------------------------------------------------
//	comStats
//	-------------------------------------------------------------------

	comStats_t gyroComStats =	 {1000*ticksPerMilliSec,};
	comStats_t vn200ComStats =	 {1000*ticksPerMilliSec,};
	comStats_t mainComStats =	 {1000*ticksPerMilliSec,};
	comStats_t outerComStats =	 { 200*ticksPerMilliSec,};
	comStats_t navboxComStats =	 {1000*ticksPerMilliSec,};
	comStats_t navCsComStats =	 {10000*ticksPerMilliSec,};
	comStats_t odinComStats =	 {1000*ticksPerMilliSec,};
	comStats_t simComStats =	 {1000*ticksPerMilliSec,};
	comStats_t util422ComStats = {1000*ticksPerMilliSec,};
	comStats_t utilUSBComStats = {1000*ticksPerMilliSec,};
	comStats_t lensComStats =	 {1000*ticksPerMilliSec,};
	comStats_t steerComStats =	 {1000*ticksPerMilliSec,};

//	-------------------------------------------------------------------
//	sequence
//	-------------------------------------------------------------------

	unsigned int stateIndex;
	unsigned int stateCount;
	int stateInitializing;
	int odinCommandWaiting;

	const float searchRate[3] = {-40,40,30};
	const float domeLimitP = 250;
	const float domeLimitN = -domeLimitP;
	const float domeRewind = -15;

//  ---------------------
//  SerialCons
//  ---------------------

	int txBufSizCons;
	#define TXBUFSIZ_CONS 2000
	#define RXBUFSIZ_CONS 2000
	unsigned char txBufCons[TXBUFSIZ_CONS];
	unsigned char rxBufCons[RXBUFSIZ_CONS];

//  ---------------------
//  SerialGyro
//  ---------------------

	#define RXBUFSIZ_GYRO 2000
	unsigned char rxBufGyro[RXBUFSIZ_GYRO];

//  ---------------------
//  SerialVN
//  ---------------------

	#define RXBUFSIZ_VN 2000
	#define TXBUFSIZ_VN 400
	unsigned char rxBufVN[RXBUFSIZ_VN];
	unsigned char txBufVN[TXBUFSIZ_VN];

//  ---------------------
//  SerialLens
//  ---------------------

	#define RXBUFSIZ_LENS 2000
	unsigned char rxBufLens[RXBUFSIZ_LENS];

//  ---------------------
//  SerialU
//  ---------------------

	#define TXUBUFSIZ 2000
	uint16_t txUhead = 0;
	uint16_t txUtail = 0;
	unsigned char txUbuf[TXUBUFSIZ];
	int txUerror = 0;

//  ---------------------
//  Jitter
//  ---------------------

	float jitBuf[NiAXES][jitCount];
	float jitInt[NiAXES];
	int jitInx[NiAXES] = {0,jitCount/2};

//	----------------------------------------------------------------------------
//	dft
//	----------------------------------------------------------------------------

	float oReal;
    float oImag;
    float iReal;
    float iImag;
    float dftT;
	const float pi = (float)PI;
	bwPar_t bwPar = {
		0,		// start
		0,		// axis
		1,		// openloop
		0,		// opendrive
		0,		// shake
		60,		// numfreqs
		.25f,	// amplitude
		5,		// minfreq
		200,	// maxfreq
	};
	bwRes_t bwRes;

//  ---------------------
//  Message buffers
//  ---------------------
									  //   header	id				 cmd count	len
	msgState_t stateMsg				  = { {HEADER,	MSG_STATE		,0	,0		,sizeof(msgState_t)			},};
	msgControls_t controlsMsg		  = { {HEADER,	MSG_CONTROLS	,0	,0		,sizeof(msgControls_t)		},};
	msgTunings_t tuningsMsg			  = { {HEADER,	MSG_TUNINGS		,0	,0		,sizeof(msgTunings_t)		},};
	msgGyroBuf_t msgGyroBuf			  = { {HEADER,	MSG_GYRO		,0	,0		,sizeof(msgGyroBuf_t)		},};
	msgBwPar_t bwMsgPar				  = { {HEADER,	MSG_BW_PAR		,0	,0		,sizeof(msgBwPar_t)			},};
	msgBwRes_t bwMsgRes				  = { {HEADER,	MSG_BW_RES		,0	,0		,sizeof(msgBwRes_t)			},};
	msgDbcmd_t msgDbcmd				  = { {HEADER,	MSG_DB_CMD		,0	,0		,0							},};
	msgLensCommand_t msgLensCommand	  = { {HEADER,	MSG_LENS_CMD	,0	,0		,sizeof(msgLensCommand_t)	},};
	msgOuterCmd_t msgOuterCmd		  = { {HEADER,	MSG_OUTER_CMD	,0	,0		,sizeof(msgOuterCmd_t)		},};
	msgOuterFb_t msgOuterFb			  = { {HEADER,	MSG_OUTER_FB	,0	,0		,sizeof(msgOuterFb_t)		},};
	msgGimToNav_t msgGimToNav		  = { {HEADER,	MSG_GIM_TO_NAV	,0	,0		,sizeof(msgGimToNav_t)		},};
	msgLog_t msgLog					  = { {HEADER,	MSG_LOG			,0	,0		,sizeof(msgLog_t)			},};
	msgGpsSim_t msgGpsSim			  = { {HEADER,	MSG_GPS_SIM		,0	,0		,sizeof(msgGpsSim_t)		},};
	msgNavToGim_t msgNavToGim		  = { {HEADER,	MSG_NAV_TO_GIM	,0	,0		,sizeof(msgNavToGim_t)		},};
	msgLensFeedback_t msgLensFeedback = { {HEADER,	MSG_LENS_FB		,0	,0		,sizeof(msgLensFeedback_t)	},};
	gyroBuffer_t gyroBuffer[2];

//  ---------------------
//  Message buffers
//  ---------------------

	const int numLog = 200;
	log_t logs[numLog];
	int logHead;
	int logTail;
	int logTailOdin;

//  ---------------------
//  PID
//  ---------------------

	float max1 = (float)0.5;

//  ---------------------
//  Calibration
//  ---------------------

	static float gain[5][NiAXES3]={
		//	Yaw				Pitch			Roll
		{	-1.0f,			-1.0f,			-1.0f			},	// gyro
		{	120.0f,			120.0f,			120.0f			},	// js
		{	kAngleGainY,	kAngleGainP,	kAngleGainR		},	// angle
		{	kTach,			kTach,			kTach			}	// tach
	};

//  ---------------------
//  PWM Pins
//  ---------------------

#if defined _OUTER

	#if  defined _004
		const int nMot = 2;
		static int pins[nMot][4]={
			{  2,  5,  6, -1 },		// Dom:	pwm, d1, d2, enable/dir
			{  3,  9, 10, -1 }		// VSW:	pwm, d1, d2, enable/dir
	};
	#endif

#elif defined _004

	const int nMot = 3;
	static int pins[nMot][4]={
		{  5,  2,  6, -1},		// Yaw:	pwm, d1, d2, enable/dir
		{  9,  3, 10, -1},		// Pit:	pwm, d1, d2, enable/dir
		{ 11,  4, 12,  1},		// Rol:	pwm, d1, d2, enable/dir
	};

#endif

//  ---------------------
//  Functions
//  ---------------------

	void analogWriteX(int pin, int duty);
	void parseMessageC(int len);
	void parseMessageU(int len);
	void parseMessageM(int len);
	void parseMessageL(int len);
//	void crcMessageC(int len);
//	void crcMessageU(int len);
	void crcMessageM(int len);
	void crcMessageL(int len);
	void jitter();
	void doPID1(int axis, int kill=0);
	void doPID2(int axis);
	void yrp();
	void yp();
	void ypr();
	void doPidOuter(int kill=0);
	void utilComms();
	void mainSend(unsigned char *msg, int len);
	int commandCheck(int cmd,int *len);
	int aiLogicFilt(int aip, aiFilter_t *filt);
	float doBw(float output);
	int isBw(int axis);
	void dftFreqs();
	void geo();
	void sequence();
	int serialGimWrite(unsigned char *msg, int len);
	int serialConsWrite(unsigned char *msg, int len);
	int serialUWrite(unsigned char *msg, int len);
	void serialUsbTx();

//	-------------------------------------------------------------------
//	Scope
//	-------------------------------------------------------------------

enum{
	scopeProc2000,
	scopeVN200,
	scopeNovAtel,
	scopeLens,
	scopeTimer3,
};

int scope = scopeProc2000;

#if defined SCOPE_PIN
	inline void scopeH(int index){if(index==scope)digitalWriteFast(SCOPE_PIN,HIGH);}
	inline void scopeL(int index){if(index==scope)digitalWriteFast(SCOPE_PIN,LOW);}
#else
	inline void scopeH(int index){}
	inline void scopeL(int index){}
#endif
//	----------------------------------------------------------------------------
//  getVolatile
//	----------------------------------------------------------------------------

inline uint16_t getVolatile(volatile uint16_t *vvar){noInterrupts(); uint16_t val = *vvar; interrupts(); return val;}
inline int32_t getVolatile(volatile int32_t *vvar){noInterrupts(); int32_t val = *vvar; interrupts(); return val;}
inline uint32_t getVolatile(volatile uint32_t *vvar){noInterrupts(); uint32_t val = *vvar; interrupts(); return val;}
inline float getVolatile(volatile float *vvar){noInterrupts(); float val = *vvar; interrupts(); return val;}

//	----------------------------------------------------------------------------
//  setVolatile
//	----------------------------------------------------------------------------

inline void setVolatile(volatile uint16_t *vvar,uint16_t val){noInterrupts(); *vvar = val; interrupts();}
inline void setVolatile(volatile int32_t *vvar,int32_t val){noInterrupts(); *vvar = val; interrupts();}
inline void setVolatile(volatile uint32_t *vvar,uint32_t val){noInterrupts(); *vvar = val; interrupts();}

//	-------------------------------------------------------------------
//	getTick
//	-------------------------------------------------------------------

inline int64_t getTick()
{
	noInterrupts();
	int64_t t = ticks;
	interrupts();
	return t;
}

//	-------------------------------------------------------------------
//	getTime
//	-------------------------------------------------------------------

float getTime()
{
	return getTick()/ticksPerSecondF;
}

//	-------------------------------------------------------------------
//	getTimeSince
//	-------------------------------------------------------------------

inline float getTimeSince(int64_t *oldTicks)
{
	int64_t t = getTick();
	float dt = (t-*oldTicks)/ticksPerSecondF;
	*oldTicks = t;
	return dt;
}

//  ---------------------------------------------------------------------------------
//  geoPar
//  ---------------------------------------------------------------------------------

#if defined(_004)

	geoPar_t geoPar[NiAXES] = {
	//	{    kP, kP2,   kI,     kD, kILim, kMystery2 }
		{ 2.00f,   0,    0,  0.01f,    20,         0 },
		{ 2.00f,   0,    0,  0.01f,    15,         0 },
		{ 1.00f,   0,    0,      0,     0,         0 }
	};

#endif

//  ---------------------------------------------------------------------------------
//  posPar
//  ---------------------------------------------------------------------------------

#if defined(_004)
	posPar_t posPar[NiAXES] = {
	//	{    kP,     kD, kLim }
		{  2.0f,  0.00f,   10 },
		{  2.0f,  0.00f,   10 },
		{ 10.0f,  0.00f,   10 }
	};
#endif

//  ---------------------------------------------------------------------------------
//  pidType
//  ---------------------------------------------------------------------------------

inline int pidType()
{
	int ipid = PID_RATE;

	switch(controls.mode){

	case MODE_RATE:
		ipid = PID_RATE;
		break;

	case MODE_GEO:
		ipid = PID_GEO;
		break;

	case MODE_SLAVE:
	case MODE_OUTER:
	case MODE_STOW:
		ipid = PID_POSN;
		break;
	}
	return ipid;
}

//  ---------------------------------------------------------------------------------
//  doPID2
//  ---------------------------------------------------------------------------------

#if defined(_INNER)

//  ---------------------------------------------------------------------------------
//  doPID2
//  ---------------------------------------------------------------------------------

void doPID2(int axis)
{
	float drive;
	int bwTest = isBw(axis);
	static float rE1[3];

	// Setup
	axisState_t *s = &(state.axis[axis]);
	pidPar_t *p = &(pidPar[axis]);
	axisControls_t *c = &(controls.axis[axis]);

	// Setup PID
	int ipid = pidType();
	float kP = p->kP[ipid];
	float kI = p->kI[ipid];
	float kD = p->kD[ipid];
	float kILim = p->kILim[ipid];

	// PID
	(s->err) = (s->rEr)*0.01f;
	(s->erI) = max( -kILim, min(kILim, (s->erI) + (kI)*(s->err) ) );
	float reD = (s->rE) - rE1[axis];
	rE1[axis] = (s->rE);
	drive = -(s->err)*(kP) - (s->erI) - (reD)*(kD);

	// Filter
	drive = filt.runfilters(drive,axis);

	// bw Test
	if(bwTest == 2){		// opendrive
		drive = doBw(s->r);
	}

	// Predicted current
	(s->current) += ((s->drive)-(s->current))*(p->kT);
	drive += ((drive)-(s->current))*(p->kC);

	// Shake Test
	if(bwTest == 3){	// shake
		drive += doBw(s->r);
	}

	// PWM Backdrive
	if((c->backdrive) == BD_DRIVE){
		float f = (c->sinFreq);
		float a = (c->sinAmp);
		float w = 2*(3.141529f)*f;
		drive = a*sinf(w*stime[axis]);
	}

	// Limit
	float lim = min(kILim,max1);
	drive = max(-lim,min(lim,drive));
	(s->drive) = drive;

	// Enable
	int standby = controls.autonull || controls.standby || (!controls.axis[axis].enable);
	if(standby){
		(s->erI) = 0;
	}

	// Output to PWM
	innerPwmOut(axis,drive,standby);

	// Jitter
	int enablepin = pins[axis][3];
	int ij = jitInx[axis];
	if(ij<jitCount){
		if((controls.standby)||(!enablepin)){
			jitInt[axis] += (s->r);
		}else{
			jitInt[axis] += (s->rE);
		}
		jitBuf[axis][ij] = jitInt[axis];
		jitInx[axis]++;
	}
}

#endif

//  ---------------------------------------------------------------------------------
//  geoPidCine
//  ---------------------------------------------------------------------------------

float geoPidCine(int axis,int *accLimOnP)
{
	// Setup
	geoPar_t *			g = &(geoPar[axis]);
	axisState_t *		s = &(state.axis[axis]);

	if(controls.mode != MODE_GEO){
		(s->geoInt) = 0;
		return 0;
	}

	float vmax = maxrate[axis];									// max rate
	float err = state.geo.losError[axis];						// position error
	float errRate = state.geo.losErrorRate[axis];				// position error rate

	// Geo PID
	float rateCommand = -(g->kP)*err;							// P
	rateCommand -= (s->geoInt);									// I
	rateCommand -= (g->kD)*errRate;								// D
	rateCommand = max(-vmax, min(vmax, rateCommand));			// rate limit
	rateCommand += state.geo.targetRate[axis];

	// Geo error lntegral
	const float dT = 1.0f/kFreq;
	float ei = (s->geoInt) + (g->kI)*err*dT;
	float imax = (g->kILim);
	ei = max(-imax,min(imax,ei));
	(s->geoInt) = ei;

	return rateCommand;
}

//  ---------------------------------------------------------------------------------
//  geoPid
//  ---------------------------------------------------------------------------------

float geoPid(int axis,int *accLimOnP)
{
	return geoPidCine(axis,accLimOnP);
}

//  ---------------------------------------------------------------------------------
//  float trig
//  ---------------------------------------------------------------------------------

	inline float cosD(float ang){ return cosf( toRadF * ang ); }
	inline float sinD(float ang){ return sinf( toRadF * ang ); }
	inline float tanD(float ang){ return tanf( toRadF * ang ); }

//  ---------------------------------------------------------------------------------
//  angles
//  ---------------------------------------------------------------------------------

void angles()
{
	static float oldAng[NiAXES];
	static float oldLosErr[NiAXES];

	float rol = state.axis[2].angle.val;
	float rolf = losFilter(2,rol);

	float pit = state.axis[1].angle.val + state.axis[1].enc.val;
	float pitf = losFilter(1,pit);

	float cospit = cosD(pitf);
	float yaw = cospit*state.axis[0].angle.val + state.axis[0].enc.val;
	float yawf = losFilter(0,yaw);

	losTach[0] = (yawf - state.axis[0].pos)*kFreq;
	losTach[1] = (pitf - state.axis[1].pos)*kFreq;
	losTach[2] = (rolf - state.axis[2].pos)*kFreq;

	state.axis[0].pos = yawf;
	state.axis[1].pos = pitf;
	state.axis[2].pos = rolf;

	for(int i=0;i<NiAXES;i++){
		float ang = state.axis[i].angle.val;
		float vel = (ang - oldAng[i])*kFreq;
		vel = innerTachFilter(i,vel);
		innerTach[i] = vel;
		oldAng[i] = ang;
	}

	// Rate
	float cosroll = cosD(rolf);
	float sinroll = sinD(rolf);
	float r0 = state.axis[0].gyro.val;
	float r1 = state.axis[1].gyro.val;
	float r2 = state.axis[2].gyro.val;
	state.axis[0].r = r0*cosroll + r1*sinroll;
	state.axis[1].r = r1*cosroll - r0*sinroll;
	state.axis[2].r = r2;

}
#if defined(_INNER)

//  ---------------------------------------------------------------------------------
//  doPID1
//  ---------------------------------------------------------------------------------

void doPID1(int axis, int kill)
{
	if(kill){
		innerPwmOut(axis,0,0);
		return;
	}

	// Setup
	axisState_t *s = &(state.axis[axis]);
	axisControls_t *c = &(controls.axis[axis]);
	geoPar_t *g = &(geoPar[axis]);
	int bwTest = isBw(axis);
	int accLimOn = 1;

	// Transform
	double pit = toRad*state.axis[1].pos;
	double rol = toRad*state.axis[2].angle.val;
	xform[0] = (float)cos(pit);

	// Inner Tach
//	float v0 = kFreq*((s->angle.val) - oldPos[axis]);
//	innerTach[axis] += (v0-innerTach[axis])*0.01f;
//	oldPos[axis] = (s->angle.val);

	// LOS Tach

//	losTach[axis] = innerTach[axis] + s->tach.val;

	// Position
//	(s->pos) = (s->angle.val) + (s->enc.val);


	// Rate Command
	float rateCommand = 0;
	float rateCommandOuter = 0;

	// Geo Rate Command
	int geoAccLimOn = accLimOn;
	float geoRateCommand = geoPid(axis,&geoAccLimOn);

	// Reset geo profile
	if(controls.mode != MODE_GEO){
		(s->profile) = 25;
		state.geo.vprofile[axis] = maxrate[axis];
	}

	if(	(controls.mode == MODE_SLAVE)||
		(controls.mode == MODE_STOW)||
		((c->backdrive) == BD_OSINE)){

		float kP = posPar[axis].kP;
		float kD = posPar[axis].kD;
		float kLim = posPar[axis].kLim;

		const float maxrateX = 100;
		
		float err = -(s->angle.val);
		err = max(-kLim, min(kLim,err));
//		rateCommand = kP * err - kD * innerTach[axis] + (s->outerDemandedRate);
//		rateCommand = kP * err - kD * innerTach[axis] + (s->tach.val);
		rateCommand = kP * err - kD * innerTach[axis];
		rateCommand = max(-maxrateX, min(maxrateX,rateCommand));
		accLimOn = 0;

	}else if((c->backdrive) == BD_STEP){

		rateCommand = step*(c->sinAmp);
		if(!(c->remote))accLimOn = 0;

	}else if((c->backdrive) == BD_SINE){

		float f = (c->sinFreq);
		float a = (c->sinAmp);
		float w = 2*(3.141529f)*f;
		rateCommand = a*sinf(w*stime[axis]);
		rateCommandOuter = rateCommand;

	}else if(bwTest == 1){

		rateCommand = doBw(s->r);

	}else if(controls.mode == MODE_GEO){

		rateCommand = geoRateCommand;
		rateCommandOuter = rateCommand;
		accLimOn = geoAccLimOn;

	}else if(c->remote){

		rateCommand = (c->desiredRate);
		rateCommandOuter = rateCommand;

	}else{
		rateCommand = 0;
		rateCommandOuter = 0;
		accLimOn = 0;
	}

	int centering_yp = (controls.mode == MODE_OUTER)?(1):(0);
	int centering_roll = 1;

	// Centering
	int centering[3]={centering_yp,centering_yp,centering_roll};

	int level = (
		(vn200ComStats.connected)&&
		(encoderValid(0))&&
		(encoderValid(1))&&
		(axis==2)
	);

	if(centering[axis]){

		float kP = posPar[axis].kP;
		float kD = posPar[axis].kD;
		float kLim = posPar[axis].kLim;
		const float maxrateX = 100;
		
		float err = 0;
		if(level){
			float p = state.axis[1].pos;
			float e = -state.geo.ypr[axis];
			if(p>-90){
				err = e*cosD(p);
			}else{
				err = -state.axis[2].angle.val;
			}
		}

		err = max(-kLim, min(kLim,err));
		rateCommand += kP * err - kD * innerTach[axis];

		int rollfix = 0;
		switch(controls.mode){
			default:
			case MODE_RATE:
				rollfix = 0;
				break;
			case MODE_SLAVE:
			case MODE_GEO:
			case MODE_OUTER:
			case MODE_STOW:
				if(axis==2)rollfix = 1;
				break;
		}
		if(rollfix){
			float sinPit = sinD(state.axis[1].pos);
			float yawrate = state.axis[0].rateCommand;
			rateCommand -= yawrate*sinPit;
		}

		rateCommand = max(-maxrateX, min(maxrateX,rateCommand));
		accLimOn = 0;

	}

#if 0
	// Inner Limits
	float angle = s->angle.val;
	float kInnerRange[3]={2,2,40};
	float maxangle = kInnerRange[axis];

	if(angle>maxangle){
//		float kD = posPar[axis].kD;
		float kP = posPar[axis].kP;
		rateCommand -= (angle-maxangle)*kP;
//		rateCommand -= kD * innerTach[axis];
		accLimOn = 0;
	}else if(angle<-maxangle){
		float kD = posPar[axis].kD;
		float kP = posPar[axis].kP;
		rateCommand -= (angle+maxangle)*kP;
//		rateCommand -= kD * innerTach[axis];
		accLimOn = 0;
	}
#endif
	// Rate
	float vmax = maxrate[axis];
	rateCommand = max(-vmax, min(vmax,rateCommand));

	// Accel Limit
	if(accLimOn){
		float amax =  maxacc2[axis];
		switch(controls.mode){
			default:
			case MODE_GEO:
			case MODE_RATE:
				amax =  maxacc2[axis];
				break;
			case MODE_SLAVE:
			case MODE_OUTER:
			case MODE_STOW:
				amax =  maxacc3[axis];
				break;
		}
		float dv = rateCommand - (s->rateCommand);
		float maxdv =amax/kFreq;
		dv = max(-maxdv, min(maxdv, dv));
		(s->rateCommand) += dv;
	}else{
		(s->rateCommand) = rateCommand;
	}

	// Rate error
	float rE =  (s->r) - (s->rateCommand);
	(s->rE) = rE;

	// Outer FF
	state_rateCommandOuter[axis] = rateCommandOuter;

}

#endif

//  ---------------------------------------------------------------------------------
//  Fill Gyro Buffer
//  ---------------------------------------------------------------------------------

void gyroLog()
{
	plotAxis = max(0,min((NiAXES-1),controls.plotAxis));

	float v;
	axisState_t *s = &(state.axis[plotAxis]);

	switch(controls.plotType){

		case PLOT_Gyro:
		default:
			v = (s->gyro.val);
			break;

		case PLOT_Command:
			v = (s->rateCommand);
			break;

		case PLOT_Angle:
			if(controls.plotRaw){
				v  = angleFilt[plotAxis].a0;
				v = gain[2][plotAxis]*(v - state.axis[plotAxis].angle.ofs);
			}else{
				v = (s->angle.val);
			}
			break;

		case PLOT_Sine:
			{
				float f = controls.axis[controls.plotAxis].sinFreq;
				float a = controls.axis[controls.plotAxis].sinAmp;
				float w = 2*(3.141529f)*f;
				v = a*sinf(w*stime[controls.plotAxis]);
			}
			break;

		case PLOT_Drive:
			v = 5.f*(s->drive);
			break;

		case PLOT_Pwm:
			v = (5.f/255.f)*(s->pwm);
			break;

		case PLOT_Current:
			v = (s->cur);
			break;

		case PLOT_Tach:
			if(controls.plotRaw){
				v = (s->tach.raw);
			}else{
				v = (s->tach.val);
			}
			break;

		case PLOT_PwmOuter:
			if(controls.plotRaw){
				v = maxOuterDriveP[plotAxis];
			}else{
				v = (s->outerDrive);
			}			
			break;

		case PLOT_NavAtt:
			v = state.nav.Ypr[plotAxis];
			break;

		case PLOT_Pos:
			v = (s->pos);
			break;	

		case PLOT_Set1:{
			int iv = sampleIndex & 0x07;
			switch(iv){
				case 0: v = (s->gyro.val);						break;
				case 1: v = (s->rateCommand);					break;
				case 2: v = state.axis[plotAxis].profile;		break;
				case 3: v = (float)(state.odin.fireCount);		break;
				case 4: v = state.geo.targetRate[plotAxis];		break;
				case 5: v = state.geo.losError[plotAxis];		break;
				case 6: v = -(s->geoInt);						break;
				case 7: v = (float)(2*state.axis[0].settled+state.axis[1].settled);				break;
			}
			break;
		}

		case PLOT_Set2:{
			int iv = sampleIndex & 0x07;
			axisState_t *s = &(state.axis[plotAxis]);
			switch(iv){
				case 0: v = (s->rateCommand);				break;
				case 1: v = (s->outerDemandedRate);			break;
				case 2: v = (s->tach.val);					break;
				case 3: v = losTach[plotAxis];				break;
				case 4: v = (s->outerRe);					break;
				case 5: v = (s->outerReI);					break;
				case 6: v = (s->outerDrive);				break;
				case 7: v = (s->angle.val);					break;
			}
			break;
		}
		case PLOT_Set3:{
			int iv = sampleIndex & 0x07;
			switch(iv){
				case 0: v = state.geo.losError[0];				break;
				case 1: v = state.geo.losError[1];				break;
				case 2: v = state.geo.losErrorRate[0];			break;
				case 3: v = state.geo.losErrorRate[1];			break;
				case 4: v = state.geo.losErrorRate2[0];			break;
				case 5: v = state.geo.losErrorRate2[1];			break;
				case 6: {
					static uint32_t oldFrameCount = state.odin.frameCount;
					int p = state.odin.frameCount - oldFrameCount;
					oldFrameCount = state.odin.frameCount;
					v = (float)p;
					break;
				}
				case 7: v = (float)state.odin.targetSeqIndex;	break;
			}
			break;
		}
		case PLOT_Set4:{
			int iv = sampleIndex & 0x07;
			int fc = state.outerCmd.focus;
			int fb = state.outerFb.focus;
			switch(iv){
				case 0: v = state.geo.losError[0];				break;
				case 1: v = state.geo.losError[1];				break;
				case 2: v = (float)state.axis[0].profile;		break;
				case 3: v = (float)fc/1000.0f;					break;
				case 4: v = (float)fb/1000.0f;					break;
				case 5: v = (float)(fc-fb);						break;
				case 6: {
					static uint32_t oldFrameCount = state.odin.frameCount;
					int p = state.odin.frameCount - oldFrameCount;
					oldFrameCount = state.odin.frameCount;
					v = (float)p;
					break;
				}
				case 7: v = (float)state.odin.targetSeqIndex;	break;
			}
			break;
		}
	}

	if(sampleIndex>=NSAMP){
		sampleHead++;
		sampleIndex = 0;
	}
	int toggle = (sampleHead & 0x01)?(1):(0);
	if(sampleIndex==0){
		gyroBuffer[toggle].timestamp = getTick();
		gyroBuffer[toggle].minval = v;
		gyroBuffer[toggle].maxval = v;
	}else{
		gyroBuffer[toggle].minval = min(gyroBuffer[toggle].minval,v);
		gyroBuffer[toggle].maxval = max(gyroBuffer[toggle].maxval,v);
	}
	gyroBuffer[toggle].samples[sampleIndex++] = v;

	if(sampleIndex==1){
		static int tt = 0;
		if(tt==5){
			step = 1;
		}else if(tt==10){
			tt = 0;
			step = -1;
		}
		tt++;
	}

	for(int i=0;i<3;i++){
		float t = stime[i] + 1.0f/kFreq;
		float freq = controls.axis[i].sinFreq;
		if(freq>0.0001f){
			float per = 1.0f/freq;
			if(t > per)t -= per;
		}
		stime[i] = t;
	}
}

//  ---------------------------------------------------------------------------------
//  aiProcess
//  ---------------------------------------------------------------------------------

float aiProcess(int aip,aiFilter_t *chan,float k1=kaFilt,float k2=kaFilt);
#if 0
float aiProcess(int aip,aiFilter_t *chan,float k1,float k2)
{
	chan->a3 = chan->a2;
	chan->a2 = chan->a1;
	chan->a1 = chan->a0;
	chan->a0 = kaScale*(analogRead(aip)-kaOfs);

	float d23 = abs(chan->a2 - chan->a3);
	float d12 = abs(chan->a1 - chan->a2);
	float d01 = abs(chan->a0 - chan->a1);

	if(d01>max(d12,d23)){
		chan->a = chan->a1;
	}else{
		chan->a = chan->a0;
	}

	if(0){
//	if(k1){
		chan->f1 += k1*(chan->a-chan->f1);
	}else{
		chan->f1 = chan->a;
	}
	if(0){
//	if(k2){
		chan->f2 += k2*(chan->f1-chan->f2);
	}else{
		chan->f2 = chan->f1;
	}
	return chan->f2;
}
#else
float aiProcess(int aip,aiFilter_t *chan,float k1,float k2)
{
	aiLogicFilt(aip,chan);
	const float dT = 3.f/kFreq;
//	const float maxd = 16.f*dT;
	const float maxd = 300*dT;
	chan->a2 += max(-maxd,min(maxd,chan->a1 - chan->a2));
	chan->a = chan->a2;
	chan->f1 = chan->a;
	chan->f2 = chan->f1;
	return chan->f2;
}
#endif

//  ---------------------------------------------------------------------------------
//  aiLogicFilt
//  ---------------------------------------------------------------------------------

int aiLogicFilt(int aip, aiFilter_t *filt)
{
	const int samplemax = 8;
	const int glitches = 4;

	int *buf = filt->aBuf;
	int b[8];
	const int invalid = 4097;

	int reading = analogRead(aip);
	buf[(filt->aInx)++] = reading;
	(filt->aInx) &= 0x07;

	memcpy(b,buf,sizeof(b));

	for(int j=0;j<glitches;j++){
	
		int mean = 0;
		int count = 0;
		for(int i=0;i<samplemax;i++){
			if(b[i]!=invalid){
				mean += b[i];
				count++;
			}
		}
		mean /= count;

		int outlier = 0;
		int maxdeviation = 0;
		for(int i=0;i<samplemax;i++){
			if(b[i]!=invalid){
				int deviation = abs(b[i]-mean);
				if(deviation>maxdeviation){
					outlier = i;
					maxdeviation = deviation;
				}
			}
		}
		b[outlier]=invalid;
	}
	int val = 0;
	int count = 0;
	for(int i=0;i<samplemax;i++){
		if(b[i]!=invalid){
			val += b[i];
			count++;
		}
	}
	val /= count;
	filt->a0 = kaScale*(reading - kaOfs);
	filt->a1 = kaScale*(val		- kaOfs);;
	return val;
}

//  ---------------------------------------------------------------------------------
//  transform
//  ---------------------------------------------------------------------------------

inline void transform()
{
	#if defined(_YPR)
		ypr();
	#else
		yp();
	#endif
}

//  ---------------------------------------------------------------------------------
//  transform1
//  ---------------------------------------------------------------------------------

void transform1()
{
	int res = 0;
	static int geotimer = 0;

	if(smooth){
		for(int i=0;i<NiAXES;i++){
			smoothRate[i] += smoothAcc[i];
		}
		smooth--;
		res = 1;
	}else{
		for(int i=0;i<NiAXES;i++){
			smoothRate[i] = steering.axis[i].desiredRate;
		}
	}

	if(steering.dynamic){
		const float kdy0 = 1.0f;
		const float kdy1 = 500.0f;
		float dyn = kdy0 + (kdy1 - kdy0)*min(1.0f,max(0.0f,steering.dynamic));
		float kdyn = 1.0f/dyn;
		for(int i=0;i<NiAXES;i++){
			dynRate[i] += kdyn*(smoothRate[i]-dynRate[i]);
		}
	}else{
		for(int i=0;i<NiAXES;i++){
			dynRate[i] = smoothRate[i];
		}
	}

	float drYaw = dynRate[0];
	float drPit = dynRate[1];
	float drRol = dynRate[2];

	if( ( steering.bits & SB_GEO_AID ) && ( state.geo.rangeP > 0.0f ) && ( controls.mode == MODE_RATE ) ){
		drYaw += state.geo.passiveRate[0];
		drPit += state.geo.passiveRate[1];
//		res = 1;
		res = 0;
		if(controls.axis[0].remote)controls.axis[0].desiredRate = drYaw;
		if(controls.axis[1].remote)controls.axis[1].desiredRate = drPit;
		if(controls.axis[2].remote)controls.axis[2].desiredRate = 0;
	}

	if( steering.bits & SB_STEER_ALT){
		double ka = 100.0/kFreq;
		double js = steering.axis[2].joystick;
		state.geo.targetL[2] +=abs(js)*js*ka;
		drRol = 0;
	}

	if(res){
		float sinPit = sinD(state.axis[1].pos);
		float cosPit = cosD(state.axis[1].pos);
		if(controls.axis[0].remote)controls.axis[0].desiredRate = drYaw*cosPit;
		if(controls.axis[1].remote)controls.axis[1].desiredRate = drPit;
		if(controls.axis[2].remote)controls.axis[2].desiredRate = drRol - drYaw*sinPit;
	}

	if(controls.mode == MODE_GEO){
		if(geotimer == 0){
			const float ks = 0.0000001f;	// m/s per deg/s at 1m
			float k = ks *state.geo.range;
			float kyaw = 1;
			float kpit = 3;
			float vy =  smoothRate[0] * k * kyaw;
			float vp = -smoothRate[1] * k * kpit;
			float yy = toRadF*state.geo.ypr[0];
			float cosy = cosf(yy);
			float siny = sinf(yy);
			float vn = vp*cosy + vy*siny;
			float ve = vy*cosy - vp*siny;
			state.geo.targetL[0] -= vn/kFreq;
			state.geo.targetL[1] += ve/kFreq;
		}else{
			geotimer--;
		}
	}else{
		geotimer = (2*kFreq)/10;
	}

}

//  ---------------------------------------------------------------------------------
//  readEncoders - Outer
//  ---------------------------------------------------------------------------------

#if defined(_OUTER)

//  ---------------------------------------------------------------------------------
//  readEncoderBits
//  ---------------------------------------------------------------------------------

inline int readEncoderBits()
{
	int in[6] = {32,31,30,36,37,38};
	int res = 0;
	int mask = 1;

	for(int i=0;i<6;i++){
		if(digitalReadFast(in[i]))res |= mask;
		mask = mask << 1;
	}
	return res;
}

//  ---------------------------------------------------------------------------------
//  readEncoders
//  ---------------------------------------------------------------------------------

inline void readEncoders(){

#define numEnc 2

	enum{
		e1bitA = 0,
		e1bitB,
		e1bitX,
		e2bitA,	
		e2bitB,
		e2bitX
	};
	enum{
		e1maskA = 1<<e1bitA,
		e1maskB = 1<<e1bitB,
		e1maskX = 1<<e1bitX,
		e2maskA = 1<<e2bitA,
		e2maskB = 1<<e2bitB,
		e2maskX = 1<<e2bitX
	};
	enum{
		e1mask = e1maskA|e1maskB|e1maskX,
		e2mask = e2maskA|e2maskB|e2maskX
	};
	enum{
		emask = e1mask|e2mask
	};

	const int tcountMax = (int)(0.05*ticksPerSecond);

	static int maskA[numEnc]={e1maskA,e2maskA};
	static int maskB[numEnc]={e1maskB,e2maskB};
	static int maskX[numEnc]={e1maskX,e2maskX};

// Count encoders

	static int portcOld=0;
	static int error[numEnc]={0,0};
	static int offset[numEnc]={0,0};

	int in1 = readEncoderBits();
	int in2 = readEncoderBits();
	for(int i=0;i<10;i++){
		int in3 = readEncoderBits();
		if((in1==in2)&&(in2==in3))break;
		in1 = in2;
		in2 = in3;
	}
	int portC = in1;
	
	int dif = portC ^ portcOld;
	portcOld = portC;
	int inc[numEnc] = {0,0};

	if(dif&emask){
		for(int i=0;i<numEnc;i++){
			int adif = dif & maskA[i];
			int bdif = dif & maskB[i];
			int xdif = dif & maskX[i];
			int a = (portC & maskA[i])?(1):(0);
			int b = (portC & maskB[i])?(1):(0);
			int x = (portC & maskX[i])?(1):(0);
			if((xdif)&&(x==0)){
				if(ecount[i])offset[i] = ecount[i];
				if(enableIndex[i]){
					if(!indexed[i]){
						ecount[i] = zerocount[i];
						bump[i] = 1;
					}
					indexed[i]++;
				}
			}else if(adif){
				if(bdif){
					error[i]++;
				}else{
					inc[i] = (a^b)?(-1):(1);
					ecount[i] += inc[i];
				}
			}else if(bdif){
				inc[i] = (a^b)?(1):(-1);
				ecount[i] += inc[i];
			}
		}
	}

// Tach

	for(int i=0;i<numEnc;i++){
		if(inc[i]){
			if(tcount[i]<tcountMax){
				if(tcount[i]>0){
					tTach[i] = inc[i]*tTachScale[i]/tcount[i];
				}
			}
			tcount[i] = 0;
		}else{
			if(tcount[i]<tcountMax){
				tcount[i]++;				// ticks since last count
			}else{
				tTach[i] = 0;				// timeout
			}
		}
	}
}

#else
	inline void readEncoders(){}
#endif

//  ---------------------------------------------------------------------------------
//  TC3_Handler
//  ---------------------------------------------------------------------------------

void TC3_Handler()
{
	scopeH(scopeTimer3);
	ticks++;
	tick1++;
	readEncoders();
	scopeL(scopeTimer3);
}

//  ------------------
//  analogWriteX
//  ------------------

void analogWriteX(int pin, int duty)
{
	analogWrite( pin, duty);
}

//  ------------------
//  setupFilters
//  ------------------

void setupFilters()
{
	for(int axis=0;axis<numInitFiltAxes;axis++){
		for(int ifil=0;ifil<numInitFilt;ifil++){
			filterPar_t *f = &filtPi[axis][ifil];
			filt.addFilter(axis, ifil, f->ftype, f->f1, f->q);
		}
	}
}

//  ------------------
//  setupPins
//  ------------------

void setupPins()
{
	// Scope
	#if defined(SCOPE_PIN)
		pinMode(SCOPE_PIN, OUTPUT);
	#endif

	// TX enable
	#if defined(TXEN_PIN)
		pinMode(TXEN_PIN, OUTPUT);
		digitalWriteFast(TXEN_PIN,1);
	#endif

	// PWM
	for(int axis=0;axis<nMot;axis++){
		if(pins[axis][3]){
			if(pins[axis][1])pinMode(pins[axis][1], OUTPUT);
			if(pins[axis][2])pinMode(pins[axis][2], OUTPUT);
			if(pins[axis][0])analogWriteFrequency(pins[axis][0], 20000);
		}
	}

	#if defined(_OUTER)	// Encoder
		pinMode(32, INPUT);			//		enc1.A			Dome
		pinMode(31, INPUT);			//		enc1.B	
		pinMode(30, INPUT);			//		enc1.X	
		pinMode(36, INPUT);			//		enc2.A			VSW
		pinMode(37, INPUT);			//		enc2.B	
		pinMode(38, INPUT);			//		enc2.X
	#endif

	#if defined(_OUTER)	// Pots
		pinMode(39, INPUT);			//		A15 yaw pot
		pinMode(40, INPUT);			//		A16 pit pot
	#else
		pinMode(38, INPUT);			//		A14 yaw pot
		pinMode(39, INPUT);			//		A15 pit pot
		pinMode(40, INPUT);			//		A16 rol pot
	#endif
}


#if defined(_OUTER)

//  -------------------
//  setupComms - Outer
//  -------------------

void setupComms()
{
	SerialGim.setTimeout(0);						// Gimbal bus (inner, outer, nav)
	SerialGim.begin(2000000);						// Gimbal bus (inner, outer, nav)
	SerialUSB.setTimeout(0);						// USB Console/Utils
	SerialUSB.begin(UTIL_BAUD);  					// USB Console/Utils
}

#else

//  -------------------
//  setupComms - 16D
//  -------------------

void setupComms()
{
	SerialGim.setTimeout(0);									// Outer
	SerialGim.begin(2000000);

	SerialCam.setTimeout(0);									// RED Camera
	SerialCam.begin(115200);

	SerialLens.setTimeout(0);									// Lens
	SerialLens.begin(2000000);
	SerialLens.addMemoryForRead(rxBufLens,sizeof(rxBufLens));

	SerialVN.setTimeout(0);										// VectorNav
	SerialVN.begin(460800);
	SerialVN.addMemoryForRead(rxBufVN,sizeof(rxBufVN));
	SerialVN.addMemoryForWrite(txBufVN,sizeof(txBufVN));

	SerialCons.setTimeout(0);									// Console/Utils
	SerialCons.begin(2*115200);
	SerialCons.addMemoryForRead(rxBufCons,sizeof(rxBufCons));
	SerialCons.addMemoryForWrite(txBufCons,sizeof(txBufCons));
	txBufSizCons = SerialCons.availableForWrite();

	SerialUSB.setTimeout(0);									// USB Console/Utils
	SerialUSB.begin(UTIL_BAUD);

	SerialGyro.setTimeout(0);									// Gyro
	#ifdef _GLADIATOR
		SerialGyro.begin( 921600,SERIAL_8E1);
	#else
		SerialGyro.begin(1000000);
	#endif
	SerialGyro.addMemoryForRead(rxBufGyro,sizeof(rxBufGyro));
}

#endif

//  ----------------
//  logBuild
//  ----------------

void logBuild()
{
	char s[80];
	sprintf(s,"scsGimbal %s SN=%03i type=%s build=%s",state.id.name, state.id.serno, typeStrings[state.id.type], build);
	toLog(s);
}

//  ---------------------
//  setupId
//  ---------------------

void setupId()
{
#if defined(_004)								// SCS Cine
	state.id.type = TYPE_S16C;
	strcpy(state.id.name,"Cine");
	state.id.serno = 4;
#else											// default
	strcpy(state.id.name,"Unknown");
	state.id.type = TYPE_16D;
	state.id.serno = 0;
#endif
	strcpy(state.id.build,SVNVER);
	toLog(" ");
	logBuild();
}

//  ----------------
//  Setup
//  ----------------

void setup()
{
	setupId();
	setupPins();
	setupComms();

	Timer1.initialize(T3PERIOD);
	Timer1.attachInterrupt(TC3_Handler);

	state.axis[0].angle.ofs = kAngleOffsY;
	state.axis[1].angle.ofs = kAngleOffsP;
	state.axis[2].angle.ofs = kAngleOffsR;

	state.axis[0].gyro.ofs = kGyroZeroY;
	state.axis[1].gyro.ofs = kGyroZeroP;
	state.axis[2].gyro.ofs = kGyroZeroR;

	setupFilters();

	analogReadResolution(12);

	controls.standby = 1;
	controls.mode = MODE_RATE;

	for(int ia=0;ia<NiAXES;ia++){
		controls.axis[ia].sinFreq = 0.5f;
		controls.axis[ia].sinAmp = 20.f;
		controls.axis[ia].enable = 1;
		controls.axis[ia].enableO = 1;
	}

#if	1
	state.geo.cameraL[0] =  43.30657850;	// Rockton
	state.geo.cameraL[1] = -80.13817753;
	state.geo.cameraL[2] = 217;
#else
	state.geo.cameraL[0] =  43.3513164;		// Office = 43.3513164 -79.7991778 109.7
	state.geo.cameraL[1] = -79.7991778;
	state.geo.cameraL[2] = 109.7;
#endif

	state.nav.PosLla[0] = state.geo.cameraL[0];
	state.nav.PosLla[1] = state.geo.cameraL[1];
	state.nav.PosLla[2] = state.geo.cameraL[2];
}

//	----------------------------------------------------------------------------
//	sendTunings
//	----------------------------------------------------------------------------

void sendTunings()
{
	for(int axis = 0;axis<NiAXES;axis++){
		tuningsMsg.t.pidP[axis] = pidPar[axis];
		tuningsMsg.t.geoP[axis] = geoPar[axis];
		tuningsMsg.t.posP[axis] = posPar[axis];
		for(int ifilt = 0;ifilt<NiFILT;ifilt++){
			filt.getFilter(axis,ifilt,&tuningsMsg.t.filtP[axis][ifilt]);
		}
	}
	for(int axis = 0;axis<NoAXES;axis++){
		tuningsMsg.t.outerP[axis] = outerPar[axis];
	}

	tuningsMsg.head.count = messagecount++;
	tuningsMsg.head.len = sizeof(tuningsMsg);
	tuningsMsg.head.id = MSG_TUNINGS;
	utilSend((unsigned char *)(&(tuningsMsg)),sizeof(tuningsMsg));
}

//	----------------------------------------------------------------------------
//	sendControls
//	----------------------------------------------------------------------------

void sendControls()
{
	controls_t *src = &controls;
	controls_t *dst = &controlsMsg.controls;
	int len = sizeof(controls_t);
	memcpy((char *)dst,(char *)src,len);
	controlsMsg.head.count = messagecount++;
	utilSend((unsigned char *)(&(controlsMsg)),sizeof(controlsMsg));
}

//	----------------------------------------------------------------------------
//	sendOuterFb
//	----------------------------------------------------------------------------

void sendOuterFb()
{
#if defined(_OUTER)
	const int baudRate = 2000000;
	const int bytes = sizeof(msgOuterFb) + 10;
	const int t0 = (10*bytes*1000000)/(T3PERIOD*baudRate);

	digitalWriteFast(TXEN_PIN,1);

	static int64_t oldTick = getTick();

	msgOuterFb.head.count = messagecount++;

	for(int i=0;i<2;i++){
		state.outerFb.tach[i] = state.axis[i].tach.val;
		state.outerFb.angle[i] = state.axis[i].angle.val;
		state.outerFb.encoder[i] = state.axis[i].enc.count;
		uint32_t indx = state.axis[i].enc.indexed;
		state.outerFb.indexed[i] = min(255,indx);
	}

	msgOuterFb.outerFb = state.outerFb;
	mainSend((unsigned char *)(&(msgOuterFb)),sizeof(msgOuterFb));

	txtimer = t0;
#endif
}

//	----------------------------------------------------------------------------
//	sendState
//	----------------------------------------------------------------------------

void sendState()
{
	stateMsg.time = getTick();
	stateMsg.state = state;
	stateMsg.head.count = messagecount++;
	utilSend((unsigned char *)(&stateMsg),sizeof(stateMsg));
}

//	----------------------------------------------------------------------------
//	sendBwPar
//	----------------------------------------------------------------------------

void sendBwPar()
{
	bwMsgPar.bwPar = bwPar;
	bwMsgPar.head.count = messagecount++;
	utilSend((unsigned char *)(&bwMsgPar),sizeof(bwMsgPar));
}

//	----------------------------------------------------------------------------
//	sendBwRes
//	----------------------------------------------------------------------------

void sendBwRes()
{
	bwMsgRes.bwRes = bwRes;
	bwMsgRes.head.count = messagecount++;
	utilSend((unsigned char *)(&bwMsgRes),sizeof(bwMsgRes));
}
//	----------------------------------------------------------------------------
//	message
//	----------------------------------------------------------------------------

unsigned char messageC[1200];
unsigned char messageU[1200];
unsigned char messageM[1200];
unsigned char messageN[1200];
unsigned char messageL[1200];
int goodCount = 0;

//	----------------------------------------------------------------------------
//	checkSum
//	----------------------------------------------------------------------------

int checkSum(int len, unsigned char *message)
{
	int len1 = len - sizeof(msgTail_t);
	msgTail_t *t1 = (msgTail_t *)&(message[len1]);;
	msgTail_t t2;
	t2.checksum = 0;
	unsigned char *msg = message;
	for(int i=0;i<len1;i++){
		t2.checksum += *msg++;
	}
	return (t2.checksum == t1->checksum)?(1):(0);
}

//	----------------------------------------------------------------------------
//	getMessageC
//	----------------------------------------------------------------------------

#if defined(SerialCons)

int getMessageC()
{
	char c;
	int res;
	static int id = 0;
	static int len = 0;
	static int index = 0;
	static int statex = 0;
	msgGyroBuf_t *g = (msgGyroBuf_t *)messageC;

	while(1){

		res = SerialCons.available();
		if(1 > res){
			return 0;
		}
		res = SerialCons.readBytes(&c,1);
		if(1 != res){
			return 0;
		}
		messageC[index++] = c;
		switch(statex){
		case 0:	// header0
			index = 0;
			messageC[index++] = c;
			if(c != 0x57){
				statex = 0;
			}else{
				statex++;
			}
			break;
		case 1:	// header1
			if(c != 0x19){
				statex = 0;
				index = 0;
			}else{
				statex++;
			}
			break;
		case  2:	// id0
		case  3:	// id1
		case  4:	// cmd0
		case  5:	// cmd1
		case  6:	// count0
		case  7:	// count1
		case  8:	// len0
		case  9:	// len1
		case 10:	// len2
			statex++;
			break;
		case 11:	// len3
			id = commandCheck(g->head.id,&len);
			if(id){
				statex++;
			}else{

				statex = 0;
				index = 0;
			}
			break;
		case 12:
			if(index>=len){
				index = 0;
				statex = 0;
				if(checkSum(len,messageC)){
					goodCount++;
					return len;
				}else{
//					crcMessageC(len);
					return 0;
				}
			}
			break;
		}
	}
}

#endif

//	----------------------------------------------------------------------------
//	getMessageU
//	----------------------------------------------------------------------------

int getMessageU()
{
	char c;
	int res;
	static int id = 0;
	static int len = 0;
	static int index = 0;
	static int statex = 0;
	msgGyroBuf_t *g = (msgGyroBuf_t *)messageU;

	while(1){

		res = SerialUtil.available();
		if(1 > res){
			return 0;
		}
		res = SerialUtil.readBytes(&c,1);
		if(1 != res){
			return 0;
		}

		messageU[index++] = c;
		switch(statex){
		case 0:	// header0
			index = 0;
			messageU[index++] = c;
			if(c != 0x57){
				statex = 0;
			}else{
				statex++;
			}
			break;
		case 1:	// header1
			if(c != 0x19){
				statex = 0;
				index = 0;
			}else{
				statex++;
			}
			break;
		case  2:	// id0
		case  3:	// id1
		case  4:	// cmd0
		case  5:	// cmd1
		case  6:	// count0
		case  7:	// count1
		case  8:	// len0
		case  9:	// len1
		case 10:	// len2
			statex++;
			break;
		case 11:	// len3
			id = commandCheck(g->head.id,&len);
			if(id){
				statex++;
			}else{
				statex = 0;
				index = 0;
			}
			break;
		case 12:
			if(index>=len){
				index = 0;
				statex = 0;
				if(checkSum(len,messageU)){
					goodCount++;
					return len;
				}else{
//					crcMessageU(len);
					return 0;
				}
			}
			break;
		}
	}
}

//	----------------------------------------------------------------------------
//	getMessageMain
//	----------------------------------------------------------------------------

int getMessageMain()
{
	char c;
	int res;
	static int id = 0;
	static int len = 0;
	static int index = 0;
	static int statex = 0;
	msgGyroBuf_t *g = (msgGyroBuf_t *)messageM;

	while(1){
		res = SerialGim.available();
		if(1 > res){
			return 0;
		}
		res = SerialGim.readBytes(&c,1);
		if(1 != res){
			return 0;
		}

		messageM[index++] = c;
		switch(statex){
		case 0:	// header0
			index = 0;
			messageM[index++] = c;
			if(c != 0x57){
				statex = 0;
			}else{
				statex++;
			}
			break;
		case 1:	// header1
			if(c != 0x19){
				statex = 0;
				index = 0;
			}else{
				statex++;
			}
			break;
		case  2:	// id0
		case  3:	// id1
		case  4:	// cmd0
		case  5:	// cmd1
		case  6:	// count0
		case  7:	// count1
		case  8:	// len0
		case  9:	// len1
		case 10:	// len2
			statex++;
			break;
		case 11:	// len3
			id = commandCheck(g->head.id,&len);
			if(id){
				statex++;
			}else{
				statex = 0;
				index = 0;
			}
			break;
		case 12:
			if(index>=len){
				index = 0;
				statex = 0;
				if(checkSum(len, messageM)){
					goodCount++;
					parseMessageM(len);
					return len;
				}else{
					crcMessageM(len);
					return 0;
				}
			}
			break;
		}
	}
}

//	----------------------------------------------------------------------------
//	getMessageNav
//	----------------------------------------------------------------------------

#if defined(SerialNav)

//	----------------------------------------------------------------------------
//	getMessageNav
//	----------------------------------------------------------------------------

int getMessageNav()
{
	char c;
	int res;
	static int id = 0;
	static int len = 0;
	static int index = 0;
	static int statex = 0;
	msgGyroBuf_t *g = (msgGyroBuf_t *)messageN;

	while(1){
		res = SerialNav.available();
		if(1 > res){
			return 0;
		}
		res = SerialNav.readBytes(&c,1);
		if(1 != res){
			return 0;
		}

		messageN[index++] = c;
		switch(statex){
		case 0:	// header0
			index = 0;
			messageN[index++] = c;
			if(c != 0x57){
				statex = 0;
			}else{
				statex++;
			}
			break;
		case 1:	// header1
			if(c != 0x19){
				statex = 0;
				index = 0;
			}else{
				statex++;
			}
			break;
		case  2:	// id0
		case  3:	// id1
		case  4:	// cmd0
		case  5:	// cmd1
		case  6:	// count0
		case  7:	// count1
		case  8:	// len0
		case  9:	// len1
		case 10:	// len2
			statex++;
			break;
		case 11:	// len3
			id = commandCheck(g->head.id,&len);
			if(id){
				statex++;
			}else{
				statex = 0;
				index = 0;
			}
			break;
		case 12:
			if(index>=len){
				index = 0;
				statex = 0;
				if(checkSum(len, messageN)){
					goodCount++;
					parseMessageN(len);
					return len;
				}else{
					crcMessageN(len);
					return 0;
				}
			}
			break;
		}
	}
}

#endif

#if defined(SerialLens)

//	----------------------------------------------------------------------------
//	getMessageLens
//	----------------------------------------------------------------------------

int getMessageLens()
{
	char c;
	int res;
	static int id = 0;
	static int len = 0;
	static int index = 0;
	static int statex = 0;
	msgGyroBuf_t *g = (msgGyroBuf_t *)messageL;

	while(1){
		res = SerialLens.available();
		if(1 > res){
			return 0;
		}
		res = SerialLens.readBytes(&c,1);
		if(1 != res){
			return 0;
		}

		messageL[index++] = c;
		switch(statex){
		case 0:	// header0
			index = 0;
			messageL[index++] = c;
			if(c != 0x57){
				statex = 0;
			}else{
				statex++;
			}
			break;
		case 1:	// header1
			if(c != 0x19){
				statex = 0;
				index = 0;
			}else{
				statex++;
			}
			break;
		case  2:	// id0
		case  3:	// id1
		case  4:	// cmd0
		case  5:	// cmd1
		case  6:	// count0
		case  7:	// count1
		case  8:	// len0
		case  9:	// len1
		case 10:	// len2
			statex++;
			break;
		case 11:	// len3
			id = commandCheck(g->head.id,&len);
			if(id){
				statex++;
			}else{
				statex = 0;
				index = 0;
			}
			break;
		case 12:
			if(index>=len){
				index = 0;
				statex = 0;
				if(checkSum(len, messageL)){
					goodCount++;
					parseMessageL(len);
					return len;
				}else{
					crcMessageL(len);
					return 0;
				}
			}
			break;
		}
	}
}

//	----------------------------------------------------------------------------
//  lensSend
//	----------------------------------------------------------------------------

void lensSend(unsigned char *msg, int len)
{
	#if defined(SerialLens)
		insertChecksum(msg,len);
		SerialLens.write(msg,len);
	#endif
}

//	-------------------------------------------------------------------
//	timeForLensCommand
//	-------------------------------------------------------------------

int timeForLensCommand()
{
	int res = 0;
	int64_t t = getTick();
	static int64_t tOld = t;
	const int64_t per = ticksPerSecond/(kLensFreq);
	if(t > tOld + per){
		res = 1;
		tOld = t;
	}
	return res;
}

//	----------------------------------------------------------------------------
//	sendLensCommand
//	----------------------------------------------------------------------------

void sendLensCommand()
{
	msgLensCommand.head.count = messagecount++;
	lensSend((unsigned char *)(&(msgLensCommand)),sizeof(msgLensCommand));
}

//	----------------------------------------------------------------------------
//  lensComms
//	----------------------------------------------------------------------------

void lensComms()
{	
	if(timeForLensCommand()){
		sendLensCommand();
	}
	getMessageLens();
}

#endif

//	----------------------------------------------------------------------------
//  
//	----------------------------------------------------------------------------

msgTail_t msgTail;

//	----------------------------------------------------------------------------
//  utilSendStart
//	----------------------------------------------------------------------------

void utilSendStart()
{
	msgTail.checksum = 0;
}

//	----------------------------------------------------------------------------
//  insertChecksum
//	----------------------------------------------------------------------------

void insertChecksum(unsigned char *msg, int len)
{
	int len1 = len - sizeof(msgTail_t);
	int16_t checksum = 0;
	unsigned char *msgx = msg;
	for(int i=0;i<len1;i++){
		checksum += *msgx++;
	}
	*msgx++ = (unsigned char)checksum;
	*msgx++ = (unsigned char)(checksum>>8);
}

//	----------------------------------------------------------------------------
//  mainSend
//	----------------------------------------------------------------------------

void mainSend(unsigned char *msg, int len)
{
	insertChecksum(msg,len);
	serialGimWrite(msg,len);
}

//	----------------------------------------------------------------------------
//  outerSend
//	----------------------------------------------------------------------------

void outerSend(unsigned char *msg, int len)
{
	insertChecksum(msg,len);
	serialGimWrite(msg,len);
}

//	----------------------------------------------------------------------------
//  navSend
//	----------------------------------------------------------------------------

void navSend(unsigned char *msg, int len)
{
#if defined(SerialNav)
	insertChecksum(msg,len);
	SerialNav.write(msg,len);
	commHit(&steerComStats);
#endif
}

//	-------------------------------------------------------------------
//	timeForOuterSend
//	-------------------------------------------------------------------

int timeForOuterSend()
{
	int res = 0;
	int64_t t = getTick();
	static int64_t tOld = t;
	const int64_t per = ticksPerSecond/100;
	if(t > tOld + per){
		res = 1;
		tOld = t;
	}
	return res;
}

//	----------------------------------------------------------------------------
//  mainComms
//	----------------------------------------------------------------------------

void mainComms()
{	
	getMessageMain();
//	if(timeForOuterSend()){
//		msgOuterFb.outerFb = state.outerFb;
//		mainSend((unsigned char *)(&(msgOuterFb)),sizeof(msgOuterFb));
//	}
}

//	----------------------------------------------------------------------------
//  utilSend
//	----------------------------------------------------------------------------

int utilSend(unsigned char *msg, int len, int addchecksum)
{
	if(addchecksum)insertChecksum(msg,len);

	if(connectUtil){
		serialUWrite(msg,len);
	}

#if defined(_INNER)
	if(connectCons){
		serialConsWrite(msg,len);
	}
#endif

  return 1;
}

//	----------------------------------------------------------------------------
//  serialConsWrite
//	----------------------------------------------------------------------------

int serialConsWrite(unsigned char *msg, int len)
{
	#if defined(SerialCons)
		SerialCons.write(msg,len);
	#endif
	return 1;
}

//	----------------------------------------------------------------------------
//  serialUWrite
//	----------------------------------------------------------------------------

int serialUWrite(unsigned char *msg, int len){
	uint16_t head = txUhead;
	unsigned char *msgx = msg;
	for(int i=0;i<len;i++){
		txUbuf[head] = *msgx++;
		if(head>(TXUBUFSIZ-2)){
			head=0;
		}else{
			head++;
		}
	}
	txUhead = head;
	return 1;
}

//	----------------------------------------------------------------------------
//  serialUsbTx
//	----------------------------------------------------------------------------

void serialUsbTx()
{
	int64_t t = getTick();
	static int64_t tlast = t;

	const int imax = 20;

	if(txUtail==txUhead)return;

	for(int i=0;i<imax;i++){
		if(txUtail==txUhead)break;
		if(SerialUSB.availableForWrite()<1){
			if((t-tlast)>1*ticksPerSecond){
				static bool utilUSB = ((void *)&SerialUSB == (void *)&SerialUtil);
				txUtail = txUhead;
				if(utilUSB)connectUtil = 0;
			}
			break;
		}else{
			tlast = t;
		}
		SerialUSB.write(txUbuf[txUtail]);
		if(txUtail>(TXUBUFSIZ-2)){
			txUtail=0;
		}else{
			txUtail++;
		}
	}
}

//	----------------------------------------------------------------------------
//  serialGimWrite
//	----------------------------------------------------------------------------

int serialGimWrite(unsigned char *msg, int len)
{
	SerialGim.write(msg,len);
	return 1;
}

//	----------------------------------------------------------------------------
//  txUEmpty
//	----------------------------------------------------------------------------

int txUEmpty()
{
	if(txUhead != txUtail)return 0;
	return 1;
}

//	----------------------------------------------------------------------------
//  utilsTxEmpty
//	----------------------------------------------------------------------------

int utilsTxEmpty()
{
	#if !defined(_OUTER)
		if(SerialCons.availableForWrite()!=txBufSizCons)return 0;
	#endif
	if(!txUEmpty())return 0;
	return 1;
}

//	----------------------------------------------------------------------------
//  utilCommsRx
//	----------------------------------------------------------------------------

void utilCommsRx()
{	
	int len;

	#if defined(SerialCons)
		len = getMessageC();
		if(len){
			parseMessageC(len);
			state.gyro.consFrame++;
		}
	#endif

	len = getMessageU();
	if(len){
		parseMessageU(len);
		state.gyro.consFrame++;
	}
}

//	----------------------------------------------------------------------------
//  utilComms
//	----------------------------------------------------------------------------

void utilComms()
{	
	utilCommsRx();

	static int toggle = 0;

//	if(!controls.connect)return;
	if(!utilsTxEmpty())return;

	if(sampleTail != sampleHead){
		sampleTail = sampleHead;
		int toggle = (sampleHead & 0x01)?(0):(1);
		msgGyroBuf.head.count = messagecount++;
		float vmin = gyroBuffer[toggle].minval;
		float vmax = gyroBuffer[toggle].maxval;
		float ks = (65535.0f)/(vmax-vmin);
		for(int i=0;i<NSAMP;i++){
			float fval = ks * (gyroBuffer[toggle].samples[i] - vmin);
			msgGyroBuf.samples[i] = (uint16_t)fval;
		}
		msgGyroBuf.timestamp = gyroBuffer[toggle].timestamp;
		msgGyroBuf.minval = vmin;
		msgGyroBuf.maxval = vmax;

		utilSend((unsigned char *)(&msgGyroBuf),sizeof(msgGyroBuf));

	}else{
		switch(toggle){
			case 0:		sendTunings();					toggle++; break;
			case 1:										toggle++; break;
			case 2:										toggle++; break;
			case 3:		sendLog();						toggle++; break;
			case 4:										toggle++; break;
			case 5:		sendControls();					toggle++; break;
			case 6:										toggle++; break;
			case 7:										toggle++; break;
			case 8:		sendState();					toggle++; break;
			case 9:										toggle++; break;
			case 10:									toggle++; break;
			case 11:	if(bwRes.bwState)sendBwRes();	toggle++; break;
			case 12:									toggle++; break;
			case 13:									toggle++; break;
			default:
				toggle = 0;
				break;
		}
	}
}

//	----------------------------------------------------------------------------
//	sendOuterCommand
//	----------------------------------------------------------------------------

void sendOuterCommand(command_t *c)
{
	static msgCommand_t msg = { HEADER,MSG_COMMAND,0,0,sizeof(msgCommand_t) };

	msg.command = *c;
	msg.tail.checksum = 0;
	outerSend((unsigned char *)(&msg), sizeof(msgCommand_t));
	msg.head.count++;
}

//	---------------------
//  parbytes_t
//	---------------------

typedef struct {
	int8_t b0;
	int8_t b1;
	int8_t b2;
	int8_t b3;
} parbytes_t;

//	----------------------------------------------------------------------------
//  sendIndexEnable
//	----------------------------------------------------------------------------

void sendIndexEnable()
{
	parbytes_t v;

	v.b0 = enableIndex[0];
	v.b1 = enableIndex[1];
	v.b2 = enableIndex[2];
	v.b3 = 0;

	int32_t iv = *(int32_t *)&v;

	command_t cmd = {CMD_ENABLE_INDEX,iv,0,0,0};
	sendOuterCommand(&cmd);
}

//	----------------------------------------------------------------------------
//  encoderValid
//	----------------------------------------------------------------------------

inline int encoderValid(int axis)
{
	return outerComStats.connected;
}

//	----------------------------------------------------------------------------
//  doPidOuter
//	----------------------------------------------------------------------------

void doPidOuter(int kill)
{
	const float dT = 1.0f/kFreq;
	const float kInnerTach[NoAXES] = {0.15f, 0.05f};
	if(fadein){
		fadein--;
	}

	for(int io=0;io<NoAXES;io++){

		axisControls_t *c = &(controls.axis[io]);
		axisState_t *s = &(state.axis[io]);
		outerPar_t *p = &(outerPar[io]);

		float rate = 0;
		float tachX = 0;

		float amax = maxaccO[io];
		float vmax = maxrate[io];

		float limN = outerStops[io][0];
		float limP = outerStops[io][1];

//		if(fadein){
//			amax = 50;
//			vmax = 50;
//		}

		if( (controls.mode == MODE_SLAVE) && (encoderValid(io)) ){
		
			const float kP = 10;
			const float kMystery3 = 0.7f;

			amax = maxacc2[io];											// max acc for slave mode
			float desiredPos = (c->desiredPos);							// desired position
			if(hasOuterStops[io]){
				desiredPos = max(limN,min(limP,desiredPos));			// apply position limits
			}
			float err = desiredPos - (s->enc.val);						// position error
			float vprofile = kMystery3*sqrtf(2*amax*fabs(err));			// accel profile velocity
			vmax = min(vmax,vprofile);									// max velocity
			rate = kP * err;											// position error term
			tachX = (s->tach.val);										// tach

		}else if( (controls.mode == MODE_STOW) && (encoderValid(io)) ){
		
			const float kP = 1;

			amax = maxacc3[io];											// max acc for stow mode
			float err = stowPos[io] - (s->enc.val);						// position error
			float vprofile = sqrtf(2*amax*fabs(err));					// accel profile velocity
			vmax = min(vmax,vprofile);									// max velocity
			rate = kP * err;											// position error term
			tachX = (s->tach.val);										// tach

		}else if(controls.mode == MODE_OUTER){
		
			if((c->backdrive) == BD_OSINE){
				float f = (c->sinFreq);
				float a = (c->sinAmp);
				float w = 2*(3.141529f)*f;
				rate = a*sinf(w*stime[io]);
			}else{
				rate = (c->desiredRate);								// rate
			}
			tachX = (s->tach.val);										// tach

		}else{
			
			float pit = state.axis[1].enc.val;
			float sinPit = sinD(pit);
			float cosPit = cosD(pit);

			float err = 0;
			float itach = 0;
			float steer = 0;

			if(io==0){

				float cosPit2 = max(-1.0f,min(1.0f,2*cosPit));			// 1.0 until 60deg
				const float k1 = 0.866f;								// sin(60)
				const float k2 = 1.0f/(1.0f-k1);						// rescale to 1
				float sinPit2 = k2*(sinPit - max(-k1,min(k1,sinPit)));	// 0.0 until 60deg

				float ayaw = state.axis[0].angle.val;
				float arol = state.axis[2].angle.val;
				const float errlim = 5.0f;
				arol = max( -errlim, min( errlim, arol ) );				// limit roll
				err = ayaw*cosPit2 - arol*sinPit2;						// resolved inner angle

				float ityaw = innerTach[0];
				float itrol = innerTach[2];
				itach = ityaw*cosPit - itrol*sinPit;					// resolved inner tach

				float syaw = state_rateCommandOuter[0];
				float srol = state_rateCommandOuter[2];
				steer = syaw*cosPit - srol*sinPit;						// resolved inner rateCommand

			}else{
				err = (s->angle.val);									// inner angle
				itach = innerTach[io];									// inner tach
				steer = state_rateCommandOuter[io];						// inner rateCommand
			}
			rate  = (p->kP)*err;										// position error
			rate += steer;												// rate ff
			tachX  = (s->tach.val);										// outer tach
			float kit = geoPar[io].kP2;									// TEMP for tuning
			if(kit == 0)kit = kInnerTach[io];
			tachX -= kit*itach;											// inner tach
		}

		rate = max(-vmax, min(vmax, rate));								// apply rate limit
		float dv = rate - (s->outerDemandedRate);						// change in demanded velocity
		float maxdv = amax*dT;											// accel limit in (deg/s)/frame
		dv = max(-maxdv, min(maxdv, dv));								// apply accel limit
		rate = (s->outerDemandedRate) + dv;

		if( (hasOuterStops[io]) && (encoderValid(io)) ){		// position limits
			float a = s->enc.val;
			if(a > limP){
				if((s->outerDemandedRate) > 0){
					rate = 0;
				}
			}else if(a < limN){
				if((s->outerDemandedRate) < 0){
					rate = 0;
				}
			}
		}
		(s->outerDemandedRate) = rate;							// final rate

		// PID
		float rE  = rate - tachX;								// rate error
		float reI = (s->outerReI) + 0.001f*(p->kI)*rE;			// rate error integral
		reI = max(-(p->kILim),min((p->kILim),reI));				// integral limit
		(s->outerRe) = rE;										// rate error
		(s->outerReI) = reI;									// rate error integral

		float drive = (p->kFF)*rate;							// proportional term
		drive = outerPwmScale(drive);							// rate proportional pwm
		drive  += (p->kTach)*rE;								// rate error term
		drive  += reI;											// integral term
		drive = max(-(p->kILim),min((p->kILim),drive));			// overall limit

		if( (!(c->enableO)) || (controls.standby) || (kill) ){
//			const float fade = dT/10;
//			drive = (s->outerDrive);
//			drive -= max(-fade,min(fade,drive));
			(s->outerDemandedRate) = 0;
			(s->outerReI) = 0;
			drive  = 0;
			fadein = kfadein;
		}

		// PWM Backdrive
		if((c->backdrive) == BD_ODRIVE){
			float f = (c->sinFreq);
			float a = (c->sinAmp);
			float w = 2*(3.141529f)*f;
			drive = a*sinf(w*stime[io]);
		}

		// PWM Limit vs tach
#if 0
		float emfdrive = (s->tach.val)/100.0f;
		float maxP = emfdrive + 1.0f;
		float maxN = emfdrive - 1.0f;
		maxP = max(0,min(max1,maxP));
		maxN = max(-max1,min(0,maxN));
		maxOuterDriveN[io] += 0.20f*(maxN - maxOuterDriveN[io]);
		maxOuterDriveP[io] += 0.20f*(maxP - maxOuterDriveP[io]);
		drive = max(maxOuterDriveN[io],min(maxOuterDriveP[io],drive));
#else
		drive = max(-max1,min(max1,drive));
#endif

		// Overspeed
		static int trip[3] = {0,0,0};
		if(fabs(s->tach.val) > kOverspeed){
			trip[io] = 5*kFreq;
		}else if(fabs(s->tach.val) < 20.0f){
			if(trip[io]){
				trip[io]--;
			}
		}
		if(trip[io]){
			const float dmax = (float)(10.0/kFreq);
			float d  = max(-max1,min(max1,-(p->kTach)*(s->tach.val)));
			float dd = d - (s->outerDrive);
			dd = max(-dmax,min(dmax,dd));
			drive = (s->outerDrive) + dd;
		}

		(s->outerDrive) = drive;
		int pwm = (int)(drive*kpwm);
		int motordrive = abs(pwm);
		int motordirection = pwm < 0;
		state.axis[io].pwmOuter = pwm;
		state.outerCmd.drive[io] = drive;
	}
#if defined(_INNER)
	static int subband = 0;
	if(subband<4){
		subband++;
		if(sendIndexEnableReq>0){
			sendIndexEnable();
			sendIndexEnableReq--;
		}
	}else {
		msgOuterCmd.cmd = state.outerCmd;
		outerSend((unsigned char *)(&msgOuterCmd), sizeof(msgOuterCmd));
		subband = 0;
	}
#endif
}

//	----------------------------------------------------------------------------
//  readGyro
//	----------------------------------------------------------------------------

#if defined(SerialGyro)

//	----------------------------------------------------------------------------
//  readGyro
//	----------------------------------------------------------------------------

#ifdef _GLADIATOR

//	----------------------------------------------------------------------------
//  readGyro
//	----------------------------------------------------------------------------

	extern int readGladiator();
	int frame;

int readGyro()
{
	int res = 0;
	if(readGladiator()){
		state.gyro.gyroFrame++;	// 	readGyro - good messages count
		commHit(&gyroComStats);
		frame++;
		if(tick1>=(1000000/T3PERIOD)){
			state.gyro.gyroFreq = frame;	// 	readGyro - good messages per sec
			frame = 0;
			tick1 = 0;
		}
		res = 1;
	}
	return res;
}

#else

//	----------------------------------------------------------------------------
//  readGyro
//	----------------------------------------------------------------------------

	const uint8_t gyroheader1 = 0x57;
	const uint8_t gyroheader2 = 0x27;
	uint8_t sum;
	uint8_t buf[9];
	int gyroIndex = 0;
	const double kGyro16 = 250./32768.;
	int frame;

int readGyro()
{
	int res = 0;

	while(SerialGyro.available()){
		uint8_t c = SerialGyro.read();
		switch(gyroIndex){
			case 0:
				if(c==gyroheader1){
					sum = c;
					buf[gyroIndex++] = c;
				}
				break;
			case 1:
				if(c==gyroheader2){
					sum += c;
					buf[gyroIndex++] = c;
				}else{
					gyroIndex = 0;
				}
				break;
			case 2:
			case 3:
			case 4:
			case 5:
			case 6:
			case 7:
				sum += c;
				buf[gyroIndex++] = c;
				break;
			case 8:
				if(sum == c){

					uint16_t lo = buf[3];
					uint16_t hi = buf[2];
					uint16_t val = lo + (hi<<8);
					int16_t v = val;
					xi2c = kGyro16*v;

					lo = buf[5];
					hi = buf[4];
					val = lo + (hi<<8);
					v = val;
					yi2c = kGyro16*v;

					lo = buf[7];
					hi = buf[6];
					val = lo + (hi<<8);
					v = val;
					zi2c = kGyro16*v;

					gyroIndex = 0;
					state.gyro.gyroFrame++;	// 	readGyro - good messages count
					commHit(&gyroComStats);
					frame++;
					if(tick1>=(1000000/T3PERIOD)){
						state.gyro.gyroFreq = frame;	// 	readGyro - good messages per sec
						frame = 0;
						tick1 = 0;
					}
					res = 1;
				}else{
					commCRC(&gyroComStats);
					gyroIndex = 0;
				}
				break;
			default:
				gyroIndex = 0;
				break;
		}
	}
	return res;
}
#endif
#endif

//	----------------------------------------------------------------------------
//  gimbalControl
//	----------------------------------------------------------------------------

#if defined(_INNER)

//	----------------------------------------------------------------------------
//  gimbalControl
//	----------------------------------------------------------------------------

void gimbalControl()
{
	static unsigned long lastGyroTime = millis();
	readVN200();
	if(readGyro()){
		lastGyroTime = millis();
		proc2000();	
	}else{
		if(!controls.standby){
			if(millis() - lastGyroTime > 5){
				killPid();
				controls.standby = 1;
			}
		}
	}
}

#endif

//	----------------------------------------------------------------------------
//  copyEncoder
//	----------------------------------------------------------------------------

void copyEncoder()
{
	noInterrupts();
	state.axis[0].enc.count = ecount[0];
	state.axis[1].enc.count = ecount[1];
	state.axis[2].enc.count = ecount[2];
	state.axis[0].enc.indexed = indexed[0];
	state.axis[1].enc.indexed = indexed[1];
	state.axis[2].enc.indexed = indexed[2];
	interrupts();
}

//	----------------------------------------------------------------------------
//	parsePidPar
//	----------------------------------------------------------------------------

void parsePidPar(int len, unsigned char *message)
{
	if(len != sizeof(msgPidPar_t))return;

	msgPidPar_t *p = (msgPidPar_t *)message;
	int axis = p->axis;
	if((axis<0)||(axis>=NiAXES))return;
	pidPar[axis] = p->pidPar;
}

//	----------------------------------------------------------------------------
//	parseTunings
//	----------------------------------------------------------------------------

void parseTunings(int len, unsigned char *message)
{
	if(len != sizeof(msgTunings_t))return;

	msgTunings_t *tt = (msgTunings_t *)message;
	tunings_t *t = &(tt->t);

	for(int axis = 0;axis<NiAXES;axis++){
//		pidPar[axis] = t->t.pidP[axis];
		geoPar[axis] = t->geoP[axis];
		posPar[axis] = t->posP[axis];
//		for(int ifilt = 0;ifilt<NiFILT;ifilt++){
//			filt.setFilter(axis,ifilt,&(t->t.filtP[axis][ifilt]));
//		}
	}

	//	for(int axis = 0;axis<NoAXES;axis++){
//		outerPar[axis] = t->t.outerP[axis];
//	}

//	for(int axis = 0;axis<NLENS;axis++){
//		lensPar[axis] = t->lensP[axis];
//	}

}

//	----------------------------------------------------------------------------
//	parseOuterPar
//	----------------------------------------------------------------------------

void parseOuterPar(int len, unsigned char *message)
{
	if(len != sizeof(msgOuterPar_t))return;

	msgOuterPar_t *p = (msgOuterPar_t *)message;
	int axis = p->axis;
	if((axis<0)||(axis>=NoAXES))return;
	outerPar[axis] = p->outerPar;
}

//	----------------------------------------------------------------------------
//	parseLensFb
//	----------------------------------------------------------------------------

void parseLensFb(int len, unsigned char *message)
{
	if(len != sizeof(msgLensFeedback_t))return;
	msgLensFeedback_t *p = (msgLensFeedback_t *)message;
	state.lens = p->lens;
}

//	----------------------------------------------------------------------------
//	parseOuterFb
//	----------------------------------------------------------------------------

void parseOuterFb(int len, unsigned char *message)
{
	if(len != sizeof(msgOuterFb_t))return;

	float ks[NoAXES] = {kDomeEnc, kVswEnc};
	float ko[NoAXES] = {kDomeOfs, kVswOfs};

	commHit(&outerComStats);

	msgOuterFb_t *p = (msgOuterFb_t *)message;
	state.outerFb = p->outerFb;

	for(int axis=0;axis<NoAXES;axis++){
		axisState_t *s = &state.axis[axis];
		s->enc.count = state.outerFb.encoder[axis];
		s->enc.indexed = state.outerFb.indexed[axis];
		s->tach.val = state.outerFb.tach[axis];
		s->outerAngle = state.outerFb.angle[axis];
		if(encoderValid(axis)){
			s->enc.val = ks[axis]*(s->enc.count) - ko[axis];
			float dif = (s->enc.val) - (s->outerAngle);
			s->outerAngleDif += 0.05f*(dif - (s->outerAngleDif) );
		}
	}
}

//	----------------------------------------------------------------------------
//	driveOuter
//	----------------------------------------------------------------------------

void driveOuter(int axis, float drive)
{
	int pwm;

	// Enable
	if(controls.axis[axis].enable){
		pwm = 2*(int)(drive*kpwm);
	}else{
		pwm = 0;
	}

	// Output to PWM
	if(pins[axis][3]){
		int motordrive = abs(pwm);
		int motordirection = (pins[axis][3]<0)?(pwm >= 0):(pwm < 0);
		analogWriteX(pins[axis][0], motordrive);
		digitalWriteFast(pins[axis][1], (motordirection) ? (0) : (1));
		digitalWriteFast(pins[axis][2], 1);
	}
	else {
		pwm = 0;
	}
	state.axis[axis].pwm = pwm;
	state.axis[axis].pwmOuter = pwm;
	state.axis[axis].drive = drive;
}

//	----------------------------------------------------------------------------
//	parseOuterCmd
//	----------------------------------------------------------------------------

void parseOuterCmd(int len, unsigned char *message)
{
#if defined(_OUTER)

	if(len != sizeof(msgOuterCmd_t))return;

	msgOuterCmd_t *p = (msgOuterCmd_t *)message;
	state.outerCmd = p->cmd;

	for(int axis=0;axis<NoAXES;axis++){
		driveOuter(axis, state.outerCmd.drive[axis]);
	}

	commHit(&outerComStats);
	sendOuterFb();
#endif
}

//	----------------------------------------------------------------------------
//	driftKnobs
//	----------------------------------------------------------------------------

void driftKnobs()
{
	static int oldEnc[2] =	{steering.enc2, steering.enc3};
	int newEnc[2] =			{steering.enc2, steering.enc3};
	int fast[2] =			{steering.buttons2 & 0x04,steering.buttons2 & 0x08};
	const float kG = 0.005f;

	if(steering.bits & SB_DRIFT_KNOB){
		for(int i=0;i<2;i++){
			float gain = (fast[i])?(10*kG):(kG);
			state.axis[i].gyro.ofs += (newEnc[i]-oldEnc[i])*gain;
			oldEnc[i] = newEnc[i];
		}
	}
}

//	----------------------------------------------------------------------------
//	parseSteering
//	----------------------------------------------------------------------------

void parseSteering(int len, unsigned char *message)
{
	if(len != sizeof(msgSteering_t))return;

	static int fpass = 1;

	if(fpass || (!steerComStats.connected) ){
		for(int i=0;i<NiAXES;i++){
			controls.axis[i].remote = 1;
		}
		fpass = 0;
	}

	steering = ((msgSteering_t *)message)->steering;

	static double oldlat = 0;
	static double oldlon = 0;
	static double oldalt = 0;

	int loadtarget = (steering.bits & SB_LOAD_TARGET)?(1):(0);

	if( (steering.mode == MODE_GEO) && (controls.mode != MODE_GEO) ){
		if( ( !(steering.bits & SB_SHIFT) ) && ( state.geo.rangeP > 0.0f ) ){
			toLog("load passive target");
			state.geo.targetL[0] = state.geo.passiveL[0];
			state.geo.targetL[1] = state.geo.passiveL[1];
			state.geo.targetL[2] = state.geo.passiveL[2];
		}else{
			//loadtarget = 1;
		}
		newGeoTargetB = 2;
	}

	if(loadtarget){
		toLog("new lat/lon/alt");
		state.geo.targetL[0] = steering.latitude;
		state.geo.targetL[1] = steering.longitude;
		state.geo.targetL[2] = steering.altitude;
		newGeoTargetB = 2;
	}

	if( steering.bits & SB_LOAD_ALT){
		toLog("new alt");
		state.geo.targetL[2] = steering.altitude;
	}

	controls.connect = steering.connect;
	controls.standby = steering.standby;
	controls.autonull = steering.autonull;
	controls.mode = steering.mode;
	controls.plotRaw = steering.plotRaw;
	controls.plotType = steering.plotType;
	controls.plotAxis = steering.plotAxis;

	const int ksmooth = 40;
	for(int i=0;i<NiAXES;i++){
		if(stateIndex == SEQ_OPERATIONAL){
			controls.axis[i].enable			= steering.axis[i].enable;
			controls.axis[i].enableO		= steering.axis[i].enableO;
			controls.axis[i].remote			= steering.axis[i].remote;
			controls.axis[i].backdrive		= steering.axis[i].backdrive;
			controls.axis[i].desiredPos		= steering.axis[i].desiredPos;
			controls.axis[i].sinAmp			= steering.axis[i].sinAmp;
			controls.axis[i].sinFreq		= steering.axis[i].sinFreq;
		}
		smoothAcc[i] = (steering.axis[i].desiredRate - smoothRate[i])/ksmooth;
	}
	smooth = ksmooth;

	msgLensCommand.focus = steering.focus;
	msgLensCommand.iris = steering.iris;
	msgLensCommand.zoom = steering.zoom;

	redControls();
	driftKnobs();

	commHit(&steerComStats);
}

//	----------------------------------------------------------------------------
//	parseBwPar
//	----------------------------------------------------------------------------

void parseBwPar(int len, unsigned char *message)
{
	if(len != sizeof(msgBwPar_t))return;

	msgBwPar_t *p = (msgBwPar_t *)message;
	bwPar = p->bwPar;
	dftFreqs();
	if(bwPar.start)bwRes.bwState =  1;
	bwPar.start = 0;
}

//	----------------------------------------------------------------------------
//	parseFiltPar
//	----------------------------------------------------------------------------

void parseFilterPar(int len, unsigned char *message)
{
	if(len != sizeof(msgFilterPar_t))return;

	msgFilterPar_t *f = (msgFilterPar_t *)message;
	int axis = f->axis;
	int ifilt = f->ifilt;
	if((ifilt<0)||(ifilt>=NiFILT))return;
	if((axis<0)||(axis>=NiAXES))return;

	filt.setFilter(axis,ifilt,&f->filtPar);
}

//	----------------------------------------------------------------------------
//	parseControls
//	----------------------------------------------------------------------------

void parseControls(int len, unsigned char *message)
{
	if(len != sizeof(msgControls_t))return;
	msgControls_t *c = (msgControls_t *)message;
	toLog("parseControls");
	if(c->controls.standby != controls.standby)toLog((c->controls.standby)?(LOG_enter_standby):(LOG_exit_standby));
	controls = c->controls;
}

//	----------------------------------------------------------------------------
//	parseCommand
//	----------------------------------------------------------------------------

void parseCommand(int len, unsigned char *message)
{
	if(len != sizeof(msgCommand_t))return;
	msgCommand_t *m = (msgCommand_t *)message;
	command_t *c = &(m->command);
	switch(c->command){
		case CMD_CONNECT_HSLOG:
			sendHsLogs = c->par1;
			toLog(sendHsLogs?LOG_HSLOG_on:LOG_HSLOG_off);
			break;
		case CMD_CONNECT:
			switch(	msgSource){
				case srcUtil:  connectUtil = c->par1;	break;
				case srcMain: connectGim = c->par1;		break;
				case srcCons: connectCons = c->par1;	break;
			}
			controls.connect = connectUtil || connectGim || connectCons || steerComStats.connected;
			break;
		case CMD_SET_HEADING:{
			#if defined(SerialVN)
				sendVNSIH((float)(c->par1));
			#endif
			break;
		}
		case CMD_ENABLE_INDEX:{
			parbytes_t *p1 = (parbytes_t *)&(c->par1);
			enableIndex[0] = p1->b0;
			enableIndex[1] = p1->b1;
			enableIndex[2] = p1->b2;
			if(!enableIndex[0])indexed[0] = 0;
			if(!enableIndex[1])indexed[1] = 0;
			if(!enableIndex[2])indexed[2] = 0;
		}
		break;
	}
}

//	----------------------------------------------------------------------------
//	Message Map
//	----------------------------------------------------------------------------

typedef struct {
	int id;
	int len;
	void (*functionPtr)(int, unsigned char *);
} messageMap_t;

messageMap_t messageMap[]={
	{MSG_GYRO,			 sizeof(msgGyroBuf_t),		0				},	// 16
	{MSG_GYRO2,			 sizeof(msgGyroBuf_t),		0				},	// 17
	{MSG_TUNINGS,		 sizeof(msgTunings_t),		parseTunings	},	// 18
	{MSG_PID_PAR,		 sizeof(msgPidPar_t),		parsePidPar		},	// 19
	{MSG_FILTER_PAR,	 sizeof(msgFilterPar_t),	parseFilterPar	},	// 20
	{MSG_CONTROLS,		 sizeof(msgControls_t),		parseControls	},	// 21
	{MSG_COMMAND,		 sizeof(msgCommand_t),		parseCommand	},	// 22
	{MSG_STATE,			 sizeof(msgState_t),		0				},	// 23
	{MSG_BW_PAR,		 sizeof(msgBwPar_t),		parseBwPar		},	// 24
	{MSG_BW_RES,		 sizeof(msgBwRes_t),		0				},	// 25
	{MSG_OUTER_PAR,		 sizeof(msgOuterPar_t),		parseOuterPar	},	// 26
	{MSG_DB_CMD,		 0,							0				},	// 27
	{MSG_UTIL,			 0,							0				},	// 28
	{MSG_CONS_INPUT,	 sizeof(msgConsInput_t),	0				},	// 29
	{MSG_LENS_CMD,		 sizeof(msgLensCommand_t),	0				},	// 30
	{MSG_LENS_FB,		 sizeof(msgLensFeedback_t),	parseLensFb		},	// 31
	{MSG_STEERING,		 sizeof(msgSteering_t),		parseSteering	},	// 32
	{MSG_OUTER_CMD,		 sizeof(msgOuterCmd_t),		parseOuterCmd	},	// 33
	{MSG_OUTER_FB,		 sizeof(msgOuterFb_t),		parseOuterFb	},	// 34
	{MSG_GIM_TO_NAV,	 sizeof(msgGimToNav_t),		0				},	// 35
	{MSG_NAV_TO_GIM,	 sizeof(msgNavToGim_t),		0				},	// 36
	{MSG_LOG,			 sizeof(msgLog_t),			0				},	// 37
	{MSG_unused,		 0,							0				},	// 38
	{MSG_NAV_to_GIM_CS,	 sizeof(msgNavBoxCS_t),		0				},	// 39
	{MSG_GPS_SIM,		 sizeof(msgGpsSim_t),		0				},	// 40
	};

//	----------------------------------------------------------------------------
//	commandCheck
//	----------------------------------------------------------------------------

int commandCheck(int cmd,int *len)
{
	if((cmd<MSG_FIRST)||(cmd>=MSG_LAST)){
		*len = 0;
		return 0;
	}

	int i = cmd-MSG_FIRST;

	if(messageMap[i].id != cmd){
		*len = 0;
		return 0;
	}
	*len = messageMap[i].len;
	return cmd;
}

//	----------------------------------------------------------------------------
//	parseMessage
//	----------------------------------------------------------------------------

inline void parseMessage(int len,unsigned char *msg, comStats_t *stats,int src)
{
	if(len<8)return;
	msgHeader_t *hdr = (msgHeader_t *)msg;
	int cmd = hdr->id;
	if(cmd<MSG_FIRST)return;
	if(cmd>=MSG_LAST)return;
	if(0 != messageMap[cmd-MSG_FIRST].functionPtr){
		msgSource = src;
		commHit(stats);
		messageMap[cmd-MSG_FIRST].functionPtr(len, msg);
	}
}

//	----------------------------------------------------------------------------
//	parseMessageC
//	----------------------------------------------------------------------------

void parseMessageC(int len)
{
	parseMessage(len,messageC,&util422ComStats,srcCons);
}

//	----------------------------------------------------------------------------
//	parseMessageU
//	----------------------------------------------------------------------------

void parseMessageU(int len)
{
	parseMessage(len,messageU,&utilUSBComStats,srcUtil);
}

//	----------------------------------------------------------------------------
//	parseMessageM
//	----------------------------------------------------------------------------

void parseMessageM(int len)
{
	state.gyro.mainFrame++;
	parseMessage(len,messageM,&mainComStats,srcMain);
}

//	----------------------------------------------------------------------------
//	crcMessageU
//	----------------------------------------------------------------------------

void crcMessageU(int len)
{
	msgGyroBuf_t *g = (msgGyroBuf_t *)messageU;
	int cmd = g->head.id;
	switch(cmd){
		case MSG_STEERING:		 commCRC(&steerComStats);	break;
	}
	 commCRC(&utilUSBComStats);
}

//	----------------------------------------------------------------------------
//	crcMessageC
//	----------------------------------------------------------------------------

	extern int16_t lastchecksum1;
	extern int16_t lastchecksum2;

void crcMessageC(int len)
{
	char s[80];
	msgSteering_t *m = (msgSteering_t *)messageC;
	int cmd = m->head.id;
	switch(cmd){
		case MSG_STEERING:
			sprintf(s,"MSG_STEERING fail %04x",
				(uint16_t)m->steering.unused);
			toLog(s);
			commCRC(&steerComStats);
			break;
	}
	commCRC(&util422ComStats);
}

//	----------------------------------------------------------------------------
//	crcMessageM
//	----------------------------------------------------------------------------

void crcMessageM(int len)
{
	commCRC(&mainComStats);
}

//	----------------------------------------------------------------------------
//	parseMessageL
//	----------------------------------------------------------------------------

void parseMessageL(int len)
{
	parseMessage(len,messageL,&lensComStats,srcLens);
}

//	----------------------------------------------------------------------------
//	crcMessageL
//	----------------------------------------------------------------------------

void crcMessageL(int len)
{
	commCRC(&lensComStats);
}

//	----------------------------------------------------------------------------
//	rms
//	----------------------------------------------------------------------------

float rms(float samples[], int count)
{
    float sum = 0;
    for (int i=0;i<count;i++)sum += samples[i]*samples[i];
    float mean = sum/count;
    return((float)sqrt(mean));
}

//	----------------------------------------------------------------------------
//	mean
//	----------------------------------------------------------------------------

float mean(float samples[], int count)
{
    float sum = 0;
    for (int i=0;i<count;i++)sum += samples[i];
    float mean = sum/count;
    return(mean);
}

//	----------------------------------------------------------------------------
//	jitter
//	----------------------------------------------------------------------------

void jitter()
{
	const float kJitterScale = (float)((1000000.0/kFreq)*(PI/180.0));

	for(int ia=0;ia<NiAXES;ia++){
		if(jitInx[ia]==jitCount){
			float m = mean(jitBuf[ia],jitCount);
			for (int i = 0;i<jitCount;i++)jitBuf[ia][i] -= m;
			state.axis[ia].jitter = rms(jitBuf[ia],jitCount)*kJitterScale;
			jitInt[ia] = 0;
			jitInx[ia] = 0;
		}
	}
}

//	----------------------------------------------------------------------------
//	dftRun
//	----------------------------------------------------------------------------

float dftRun(float output)
{

	float s;
	float c;
	float gain;
	float phase;
	float input;
	float outputOld = bwRes.output;

	s = sinf(dftT);
	c = cosf(dftT);
	dftT += (2*pi*bwRes.freq[bwRes.index])/kFreq;

	input = bwPar.amplitude*s;
	bwRes.drive = input;
	bwRes.output = output;

	if(bwPar.openloop){
		input -= output;
	}

	if(bwPar.opendrive){
		output -= outputOld;	//	acceleration
	}

	iReal += s*input;
	iImag -= c*input;
	oReal += s*output;
	oImag -= c*output;

	gain = sqrtf((oReal*oReal + oImag*oImag)/(iReal*iReal + iImag*iImag));

	phase = atan2f(iImag, iReal) - atan2f(oImag, oReal);
	if(phase > 5*(pi/180))phase -= 2*pi;

	bwRes.gain[bwRes.index] = gain;
	bwRes.phase[bwRes.index] = phase*(180.f/pi);

	return bwRes.drive;
}

//	----------------------------------------------------------------------------
//	dftFreqs
//	----------------------------------------------------------------------------

void dftFreqs()
{
	float lfmin = (float)log(bwPar.minfreq);
	float lfmax = (float)log(bwPar.maxfreq);
	float kfreq = (lfmax-lfmin)/(NUMBW-1);
	for (int i=0; i < NUMBW; i++){
		float logfreq = lfmin + i*kfreq;
		bwRes.freq[i] = (float)exp(logfreq);
		bwRes.gain[i] = 0;
		bwRes.phase[i] = 0;
	}
}

//	----------------------------------------------------------------------------
//	dftInit
//	----------------------------------------------------------------------------

void dftInit()
{
	oReal = 0.0;
    oImag = 0.0;
    iReal = 0.0;
    iImag = 0.0;
    dftT = 0.0;
	if(bwRes.freq[0]==0){
		dftFreqs();
	}
}

//	----------------------------------------------------------------------------
//	doBw
//	----------------------------------------------------------------------------

float doBw(float output)
{
	float res = 0;

	switch(bwRes.bwState){
		case 0:
			break;
		case 1:
			dftInit();
			bwRes.index = 0;
			bwRes.bwState++;
			break;
		case 2:
			if(dftT > 20*2*pi){
				dftInit();
				bwRes.index++;
				if(bwRes.index>(NUMBW-1)){
					bwRes.bwState++;
				}
			}else{
				res = dftRun(output);
			}
			break;
		case 3:
			break;
	}
	return res;
}

//	----------------------------------------------------------------------------
//	isBw
//	----------------------------------------------------------------------------

int isBw(int axis)
{
	if(axis != (int)bwPar.axis)return 0;
	if(bwRes.bwState < 1)return 0;
	if(bwRes.bwState > 2)return 0;
	if(bwPar.shake)return 3;
	if(bwPar.opendrive)return 2;
	return 1;
}

//	----------------------------------------------------------------------------
//	readVN200
//	----------------------------------------------------------------------------

#if defined(SerialVN)

//	----------------------------------------------------------------------------
//	readVN200
//	----------------------------------------------------------------------------

int readVN200()
{
	int rval = 0;
	static uint8_t header[] = 
{
	0xFA, 0x35,
	0xEF, 0x10,
	0x11, 0x00,
	0x00, 0x01,
	0x00, 0x06
};

	const int navSiz = sizeof(navState_t);
	const int crcLen = 2;
	const int dummyReserveBufSize = 10;
	const int headerSize = 10;
	const int totalMsgSize = navSiz + headerSize + crcLen;
	static uint8_t navbuf[totalMsgSize + dummyReserveBufSize];
	static int navIndex = 0;
	static int navPtr = 0;

	while (SerialVN.available()) {

		uint8_t c = SerialVN.read();
		
		switch(navIndex) {

			case 0:
			case 1:
			case 2:
			case 3:
			case 4:
			case 5:
			case 6:
			case 7:
				if(c == header[navIndex]) {
					navbuf[navIndex++] = c;
				}else {
					navIndex = 0;
				}
				navPtr = navIndex;
				break;

			case 8:

				if(navPtr < totalMsgSize){
					navbuf[navPtr++] = c;
				}

				if (navPtr >= totalMsgSize){

					const int crcedPayloadLength = navSiz + headerSize - 1;
					const int bufferToCrcStart = 1;
					unsigned short crc = calculateCRC(&navbuf[bufferToCrcStart], crcedPayloadLength);
				
					bool crcOk = //crc == *(uint16_t*)&navbuf[navSiz + headerSize];
						((crc >> 8) == navbuf[navSiz + headerSize]) &&
						((crc & 0xff) == navbuf[navSiz + headerSize + 1]);

					crcOk = true;

					if(crcOk){
						const int payload = headerSize;
						navState_t *nav = (navState_t*)&navbuf[payload];
						parseVN200(nav);
						rval = 1;
					}else{
						commCRC(&vn200ComStats);
					}
					navIndex = 0;
					navPtr = 0;
				}
				break;
			default:
				navIndex = 0;
				navPtr = 0;
		}
	}
	return rval;
}

//	----------------------------------------------------------------------------------------------------
//	calculateCRC - from VN200 manual "Calculates the 16-bit CRC for the given ASCII or binary message."
//	----------------------------------------------------------------------------------------------------

unsigned short calculateCRC(unsigned char data[], unsigned int length)
{
	unsigned int i;
	unsigned short crc = 0;
	for(i=0; i<length; i++){
		crc = (unsigned char)(crc >> 8) | (crc << 8);
		crc ^= data[i];
		crc ^= (unsigned char)(crc & 0xff) >> 4;
		crc ^= crc << 12;
		crc ^= (crc & 0x00ff) << 5;
	}
	return crc;
}

//	----------------------------------------------------------------------------
//  appendAsciiCrc16
//	----------------------------------------------------------------------------

int appendAsciiCrc16(char *buffer,int len) //, int maxlen)
{
	uint16_t crc = calculateCRC((unsigned char*)(&buffer[1]),len-1);
//	len += sprintf(&buffer[len], "*%04X", crc);
	len += sprintf(&buffer[len], "*%04X\r\n", crc);
	return len;
}

//	-------------------------------------------------------------------
//	VN-200 set initial heading
//	-------------------------------------------------------------------

void sendVNSIH(float heading){

	const int vnmsgLen = 30;
	char vnmsg[vnmsgLen];
	int len;

	len = sprintf(vnmsg,"$VNSIH,%1.2f",heading);
	len = appendAsciiCrc16(vnmsg,len);
	SerialVN.write(vnmsg,len);
	toLog(vnmsg,LOG_sendVNSIH);
}

//	-------------------------------------------------------------------
//	sendVNSetup
//	-------------------------------------------------------------------

void sendVNSetup()
{
	const int nm = 3;
	const char *m[nm] = {
		"$VNWRG,57,+0.000,-0.3048,+0.000*XXXX\r\n",																			// antenna 1 offset
		"$VNWRG,93,+0.000,+0.6096,+0.000,+0.0254,+0.0254,+0.0254*XXXX\r\n",													// antenna 2 offset and uncertainty
		"$VNWRG,26,-0.9702957,0.2419219,0.0000000,-0.2419219,-0.9702957,0.0000000,0.0000000,0.0000000,1.0000000*XXXX\r\n"	// IMU orientation
//		"$VNWRG,26,-0.01,0.99,0.0,-0.99,-0.01,0.0,0.0,0.0,1.0*XXXX\r\n"	// IMU orientation
	};

	for(int i=0;i<nm;i++){
		SerialVN.write((uint8_t *)m[i],(int)strlen(m[i]));
//		toLog(m[i],LOG_sendVNSIH);
	}
}

#endif

//	---------------------
//  A1minusA2
//	---------------------

inline float A1minusA2(float a1,float a2)
{
	float rval = a1 - a2;
	if(rval>180){
		rval-=360;
	} else if(rval<-180){
		rval+=360;
	}
	return rval;
}

//	---------------------
//  geo2
//	---------------------

void geo2()
{
	geodeticToEcef(&state.geo.cameraL, &geoMisc2.cameraE);
	geodeticToEcef(&state.geo.targetL, &geoMisc2.targetE);
	for (int i = 0; i < 3; i++) {
		geoMisc2.vectorE[i] = geoMisc2.targetE[i] - geoMisc2.cameraE[i];
	}
	earthToRe2t(&state.geo.cameraL, &geoMisc2.Re2t);
	mult3by3times3by1(&geoMisc2.Re2t, &geoMisc2.vectorE, &geoMisc2.vectorT);

	double ypr[3] = { state.nav.Ypr[0],state.nav.Ypr[1],state.nav.Ypr[2] };
	yprEulerToRot(&ypr, &geoMisc2.c);
	mult3by3times3by1(&geoMisc2.c, &geoMisc2.vectorT, &geoMisc2.vectorC);
	state.geo.losError2[0] = (float)(toDeg * atan2(-geoMisc2.vectorC[1], geoMisc2.vectorC[0]));
	state.geo.losError2[1] = (float)(toDeg * atan2(geoMisc2.vectorC[2], geoMisc2.vectorC[0]));
	state.geo.losError2[2] = state.nav.Ypr[2];

	const float dT = 1.0f/200;
	static float e2Old[3]={state.geo.losError2[0],state.geo.losError2[1],state.geo.losError2[2]};
	for(int i=0;i<NiAXES;i++){
		state.geo.losErrorRate2[i] = A1minusA2(state.geo.losError2[i],e2Old[i])/dT;
		e2Old[i] = state.geo.losError2[i];
	}
	static float rangeOld = state.geo.range;
	state.geo.rangeRate = (state.geo.range-rangeOld)/dT;
	rangeOld = state.geo.range;
}

//	----------------------------------------------------------------------------
//	simVN200
//	----------------------------------------------------------------------------

void simVN200(navState_t *_navState)
{
	const float dTF = 1.0f/100;				// s/frame
	const double dT = dTF;					// s/frame

	double speed = steering.sim.speed;		// m/s

	static double dLat = 0;
	static double dLon = 0;
	static double dAlt = 0;
	static double range12 = 0;

	static double lat = 0;
	static double lon = 0;
	static double alt = 0;
	static double lat1 = 0;
	static double lon1 = 0;
	static double alt1 = 0;
	static double lat2 = 0;
	static double lon2 = 0;
	static double alt2 = 0;

	double range;

	static double velN;
	static double velE;
	static double velU;

	static float hdg = 0;
	static float heading = 0;

	navState_t navState = *_navState;

	int init = 0;
	int set = 0;

	static int oldmode = SIM_MODE_OFF;
	if(steering.sim.mode!=oldmode){
		oldmode = steering.sim.mode;
		switch(steering.sim.mode){
			case SIM_MODE_OFF:		toLog("Sim Mode = SIM_MODE_OFF"); break;
			case SIM_MODE_SET:		toLog("Sim Mode = SIM_MODE_SET"); break;
			case SIM_MODE_RUN:		toLog("Sim Mode = SIM_MODE_RUN"); break;
			default:				toLog("Sim Mode = Unknown"); break;
		}
		set = 1;
	}

	if(set){

		lat1 = steering.sim.lla1[0]; lon1 = steering.sim.lla1[1]; alt1 = steering.sim.lla1[2];
		lat2 = steering.sim.lla2[0]; lon2 = steering.sim.lla2[1]; alt2 = steering.sim.lla2[2];
		init = 1;

	}
	
	if( (steering.sim.mode == SIM_MODE_RUN) && (!init) && (range12 > 0) ){

		lat += dLat;
		lon += dLon;
		alt += dAlt;

		double dN;
		double dE;
		double dA = alt2 - alt1;
		latLonToMeters(lat,lon,&dN,&dE,lat1,lon1);
		range = sqrt( dN*dN + dE*dE + dA*dA );

		if(range >= range12){
			double la1 = lat1;
			double lo1 = lon1;
			double al1 = alt1;
			lat1 = lat2;
			lon1 = lon2;
			alt1 = alt2;
			lat2 = la1;
			lon2 = lo1;
			alt2 = al1;
			init = 1;
		}
	}

	if(init){

		lat = lat1;
		lon = lon1;
		alt = alt1;
	
		double dN;
		double dE;
		latLonToMeters(lat2,lon2,&dE,&dN,lat1,lon1);
		double dA = alt2 - alt1;
		range12 = sqrt( dN*dN + dE*dE + dA*dA );
		double dNE = sqrt( dN*dN + dE*dE );

		if((speed>0.1f)&&(range12>1.0f)){
			double time12 = dNE/speed;
			velN = speed*(dN/dNE);
			velE = speed*(dE/dNE);
			velU = speed*(dA/dNE);
		}else{
			velN = 0;
			velE = 0;
			velU = 0;
		}

		hdg = (float)(toDeg*atan2(dE,dN));

		double kN = (lat2-lat1)/dN;
		double kE = (lon2-lon1)/dE;
		dLat = velN*kN*dT;
		dLon = velE*kE*dT;
		dAlt = velU*dT;

		init = 0;


	}

	if( steering.sim.mode == SIM_MODE_RUN ){

		float dhmax = 60.0f*dTF;
		heading += min(dhmax,max(-dhmax,hdg - heading));

		navState.PosLla[0] = lat;
		navState.PosLla[1] = lon;
		navState.PosLla[2] = alt;
		navState.Ypr[0] = heading;
		navState.VelNed[0] =  (float)( velN);
		navState.VelNed[1] =  (float)( velE);
		navState.VelNed[2] =  (float)(-velU);

		state.nav = navState;
	}
}

//	----------------------------------------------------------------------------
//	parseVN200
//	----------------------------------------------------------------------------

//	----------------------------------------------------------------------------
//	parseVN200
//	----------------------------------------------------------------------------

void parseVN200(navState_t *_navState)
{
	commHit(&vn200ComStats);
	vn200Connected = 1;

	if(steering.sim.mode == SIM_MODE_OFF){
		state.nav = *_navState;
	}
	simVN200(_navState);
	geo();

//	static int fpass = 1;
//	if(fpass){
//		void sendVNSetup();
//		sendVNSetup();
//		fpass = 0;
//	}
}

//	---------------------
//  geo
//	---------------------

float fYawHdgT;		// simulated AC yaw

//	---------------------
//  geo
//	---------------------

void geo()
{
	const float dT = 1.0f / 100;
	static float freqRatio = 11;
	static int ratio = 1;
	static int upsample = 0;
	static double dLLA[3] = { 0,0,0 };
	static float dHdg = 0;

	int good = state.nav.PosLla[0] != 0;
	if (good) {
		for (int i = 0; i < 3; i++) {
			state.geo.cameraL[i] = state.nav.PosLla[i];
		}
	}
	boresight(); // state.geo.ypr = f(state.nav.Ypr)
	geodeticToEcef(&state.geo.cameraL, &geoMisc.cameraE);
	geodeticToEcef(&state.geo.targetL, &geoMisc.targetE);
	for (int i = 0; i < 3; i++) {
		geoMisc.vectorE[i] = geoMisc.targetE[i] - geoMisc.cameraE[i];
	}
	state.geo.range = (float)sqrt(
		geoMisc.vectorE[N] * geoMisc.vectorE[N] +
		geoMisc.vectorE[E] * geoMisc.vectorE[E] +
		geoMisc.vectorE[D] * geoMisc.vectorE[D]);
	static double latO = 0;
	static double lonO = 0;
	if((abs(state.geo.cameraL[0]-latO)>1)||(abs(state.geo.cameraL[1]-lonO)>1)){
		latO = state.geo.cameraL[0];
		lonO = state.geo.cameraL[1];
		earthToRe2t(&state.geo.cameraL, &geoMisc.Re2t);
	}
	mult3by3times3by1(&geoMisc.Re2t, &geoMisc.vectorE, &geoMisc.vectorT);

	float yEarth = (float)state.geo.ypr[0];
	float pEarth = (float)state.geo.ypr[1];
	float rEarth = (float)state.geo.ypr[2];

	float yErr = A1minusA2(yEarth, state.geo.targetAngle[0]);
	float pErr = A1minusA2(pEarth, state.geo.targetAngle[1]);

	if (hasOuterStops[0]) {						// don't wrap past yaw stops

		float limN = outerStops[0][0];
		float limP = outerStops[0][1];

		float curYaw = state.axis[0].pos;
		float geoYaw = curYaw - yErr;

		if (geoYaw < limN) {
			if ((geoYaw + 360) < limP)geoYaw += 360;
		}
		else if (geoYaw > limP) {
			if ((geoYaw - 360) > limN)geoYaw -= 360;
		}
		geoYaw = max(limN, min(limP, geoYaw));
		float yErr2 = curYaw - geoYaw;
		state.geo.losError[0] = geoFilter(0, yErr2 * xform[0]);
	}
	else {
		state.geo.losError[0] = geoFilter(0, yErr * xform[0]);
	}

	state.geo.losError[1] = geoFilter(1, pErr);
	state.geo.losError[2] = geoFilter(2, rEarth);

	float e[3] = { yErr * xform[0],pErr,rEarth };
	static float eOld[3] = { e[0],e[1],e[2] };
	for (int i = 0; i < NiAXES; i++) {
		state.geo.losErrorRate[i] = A1minusA2(e[i], eOld[i]) / dT;
		eOld[i] = e[i];
	}

	for (int axis = 0; axis < NiAXES; axis++) {
		float v = ((float)toDeg) * (state.nav.AngularRate[2 - axis]);
		//		if( fabs(v) < 5.0f ){
		//			navOffset[axis] = v - (gain[0][axis])*(state.axis[axis].gyro.raw);
		//			state.axis[axis].gyro.ofs = - navOffset[axis];
		//		}
		navRate[axis] = v;
	}
	passive();
	passiveRate();

	float pN = (float)geoMisc.vectorT[0];
	float pE = (float)geoMisc.vectorT[1];
	float pD = (float)geoMisc.vectorT[2];

	float neRangeF = sqrt(pN * pN + pE * pE);

	float yTarg = toDegF * atan2f(pE, pN);
	float pTarg = toDegF * atan2f(-pD, neRangeF);

	static int sub10 = 0;
	//	const int ksub10 = 100-1;	// 1hz
	const int ksub10 = 10;		// 10.0hz
	static float yTargOld = yTarg;
	static float pTargOld = pTarg;
	int64_t tic = getTick();
	static int64_t ticOld = tic;
	pTargOld = pTarg;
	if (sub10 >= ksub10) {
		sub10 = 0;
		if (newGeoTargetB > 0) {
			newGeoTargetB--;
		}
		else {
			float dT10 = (tic - ticOld)/ticksPerSecondF;

//			state.geo.targetRate[0] = xform[0] * (A1minusA2(yTarg, yTargOld)) / (ksub10 * dT);
//			state.geo.targetRate[1] = xform[1] * (A1minusA2(pTarg, pTargOld)) / (ksub10 * dT);
			int idT10 = (int)(1000*dT10);
			state.gyro.misc3 = idT10;
//			if(idT10){
			if(0){
				state.geo.targetRate[0] = (A1minusA2(yTarg, yTargOld)) / dT10;
				state.geo.targetRate[1] = (A1minusA2(pTarg, pTargOld)) / dT10;
			}
		}
		yTargOld = yTarg;
		pTargOld = pTarg;
		ticOld = tic;
	}
	sub10++;

	state.geo.targetAngle[0] = yTarg;
	state.geo.targetAngle[1] = pTarg;
}

//	---------------------
//  yp
//	---------------------

void yp()
{
	for(int ia=0;ia<NiAXES;ia++){
		state.axis[ia].rEr = state.axis[ia].rE;
	}
}

//	---------------------
//  yrp
//	---------------------

void yrp()
{
	float j[3][3];
	const float pi = 3.1415926f;
	const float toRad = pi/180.0f;

	float pit = toRad*state.axis[1].pos;
	float rol = toRad*state.axis[2].pos;

	float sinPitch = sin(pit);
	float cosPitch = cos(pit);

	float cosRoll = cos(rol);
	float tanRoll = tan(rol);

	j[2][2] = cosPitch;
	j[2][1] = 0;
	j[2][0] = sinPitch;
	j[1][2] = sinPitch*tanRoll;
	j[1][1] = 1;
	j[1][0] = -cosPitch*tanRoll;
	j[0][2] = -sinPitch/cosRoll;
	j[0][1] = 0;
	j[0][0] = cosPitch/cosRoll;

	float rE[3] = {state.axis[0].rE,state.axis[1].rE,state.axis[2].rE};
	float rEr[3];
	mult3by3times3by1(&j,&rE,&rEr);
	state.axis[0].rEr = rEr[0];
	state.axis[1].rEr = rEr[1];
	state.axis[2].rEr = rEr[2];
}

//	---------------------
//  ypr
//	---------------------

void ypr()
{
	float pit = (float)toRad*state.axis[1].pos;
	float sinPitch = sin(pit);

	state.axis[0].rEr = state.axis[0].rE;
	state.axis[1].rEr = state.axis[1].rE;
//	state.axis[2].rEr = state.axis[2].rE + sinPitch*state.axis[0].tach.val;
	state.axis[2].rEr = state.axis[2].rE;
}

//	-------------------------------------------------------------------
//	sequenceTimer
//	-------------------------------------------------------------------

const int stateFreq = 200;

int sequenceTimer()
{	
	const int f = stateFreq;
	const int64_t dT = 1000000/(f*T3PERIOD);

	static int64_t t = 0;
	int64_t tick0 = getTick();
	static int64_t tick1 = tick0;
	t += tick0 - tick1;
	tick1 = tick0;
	if(t<dT)return 0;
	t -= dT;
	if((t>dT)||(t<0))t = 0;
	return 1;
}

//	-------------------------------------------------------------------
//	sequence
//	-------------------------------------------------------------------

void sequence()
{

	switch(stateIndex){

		case SEQ_INIT:
			controls.standby = 1;
			stateIndex = SEQ_OPERATIONAL;
			break;
	}
	return;
}

//	----------------------------------------------------------------------------
//  killCheck
//	----------------------------------------------------------------------------

void killCheck()
{
#if defined(_OUTER)
	if(!outerComStats.connected){
		for(int axis=0;axis<NoAXES;axis++){
			driveOuter(axis, 0);
		}
	}
#endif
}

//	----------------------------------------------------------------------------
//  Main Process
//	----------------------------------------------------------------------------

void loop()
{
	ticksCommon = getTick();

	#if defined(_OUTER)
		copyEncoder();
		mainComms();
		killCheck();
		readOuterAnalog();

	#else
		sequence();
		gimbalControl();
		mainComms();
		lensComms();
	#endif

	utilComms();
	monitor();
	serialUsbTx();

}

//	---------------------------------------------------------------------------------------------------
//	calculateChecksum - from VN200 manual "Calculates the 8-bit checksum for the given byte sequence."
//	---------------------------------------------------------------------------------------------------

unsigned char calculateChecksum(unsigned char data[], unsigned int length)
{
	unsigned int i;
	unsigned char cksum = 0;
	for(i=0; i<length; i++){
		cksum ^= data[i];
	}
	return cksum;
}

//	-------------------------------------------------------------------
//	comStatS
//	-------------------------------------------------------------------

	const int numComStats = 12;
	comStats_t *comStatS[numComStats] = 
	{
		&gyroComStats,
		&vn200ComStats,
		&mainComStats,
		&outerComStats,
		&navboxComStats,
		&navCsComStats,
		&odinComStats,
		&simComStats,
		&util422ComStats,
		&utilUSBComStats,
		&lensComStats,
		&steerComStats
	};

//	-------------------------------------------------------------------
//	comStatS
//	-------------------------------------------------------------------

	int32_t *comStateS[numComStats] = 
	{
		&state.comm[0].gyro,
		&state.comm[0].vn200,
		&state.comm[0].main422,
		&state.comm[0].outer,
		&state.comm[0].navbox,
		&state.comm[0].navCs,
		&state.comm[0].odin,
		&state.comm[0].sim,
		&state.comm[0].console,
		&state.comm[0].util,
		&state.comm[0].lens,
		&state.comm[0].steering
	};

//	-------------------------------------------------------------------
//	checkCommTimeout
//	-------------------------------------------------------------------

inline int checkCommTimeout(comStats_t *s,int64_t tpresent)
{
	if(s->connected){
		int64_t ttimeout = (s->lastRx) + (s->kTimeout);
		if(tpresent > ttimeout){
			(s->connected) = 0;
		}
	}
	return (s->connected);
}

//	-------------------------------------------------------------------
//	commHit
//	-------------------------------------------------------------------

void commHit(comStats_t *s)
{
	(s->lastRx) = getTick();
	(s->connected) = 1;
	(s->count)++;
	(s->count2)++;
}

//	-------------------------------------------------------------------
//	commCRC
//	-------------------------------------------------------------------

void commCRC(comStats_t *s)
{
	(s->crcCount)++;
	(s->crcCount2)++;
}

//	-------------------------------------------------------------------
//	comStatsCheck
//	-------------------------------------------------------------------

void comStatsCheck()
{
	int64_t tpresent = getTick();
	static int64_t tlastSample = tpresent;
	const int numSec = 3;
	const int64_t ktsample = numSec*ticksPerSecond;
	const int64_t kt = ticksPerSecond;
	const int64_t roundup =  ktsample/2;
		
	for(int i=0;i<numComStats;i++){
		checkCommTimeout(comStatS[i],tpresent);
	}

	int32_t tdelta = (int32_t)(tpresent - tlastSample);
	if(tdelta>ktsample){
		for(int i=0;i<numComStats;i++){
			(comStatS[i]->countsPerSec) = (kt*(comStatS[i]->count2   ) + roundup)/tdelta;
			(comStatS[i]->crcPerSec) =    (kt*(comStatS[i]->crcCount2) + roundup)/tdelta;
			(comStatS[i]->count2) = 0;
			(comStatS[i]->crcCount2) = 0;
		}
		tlastSample = tpresent;
	}
}

//	-------------------------------------------------------------------
//	monitor
//	-------------------------------------------------------------------

void monitor()
{
	comStatsCheck();

	for(int i=0;i<numComStats;i++){
		int32_t *dest = comStateS[i];
		dest[0*numComStats] = comStatS[i]->countsPerSec;
		dest[1*numComStats] = comStatS[i]->crcPerSec;
		dest[2*numComStats] = comStatS[i]->crcCount;
	}

	odinSim = simComStats.connected;
	odinConnected = odinComStats.connected;
}

//	-------------------------------------------------------------------
//	strCpy
//	-------------------------------------------------------------------

void strCpy(char *d,int len,const char *s)
{
	int slen = (int)strlen(s);
	if(slen > (len-1)){
		memcpy(d,s,len);
		d[len-1] = 0;
	}else{
		strcpy(d,s);
	}
}

//	-------------------------------------------------------------------
//	toLog
//	-------------------------------------------------------------------

void toLog(uint32_t type)
{
	if(type>=NUM_LOG_TYPES)return;
	toLog(logStrings[type],type);
}

//	-------------------------------------------------------------------
//	toLog
//	-------------------------------------------------------------------

void toLog(const char *s, uint32_t type)
{
	int nexthead = logHead + 1;
	if(nexthead >= numLog)nexthead = 0;
	logs[logHead].time = getTick();
	logs[logHead].type = type;
	strCpy(logs[logHead].s,logstringlen,s);
	logHead = nexthead;
}

//	-------------------------------------------------------------------
//	sendLog
//	-------------------------------------------------------------------

void sendLog()
{
	if(logHead == logTail)return;

	int nexttail = logTail + 1;
	if(nexttail >= numLog)nexttail = 0;
	msgLog.entry = logs[logTail];
	utilSend((unsigned char *)(&(msgLog)),sizeof(msgLog));
	logTail = nexttail;
}


//	-------------------------------------------------------------------
//	timeForOuterAnalog
//	-------------------------------------------------------------------

int timeForOuterAnalog()
{
	int res = 0;
	int64_t t = getTick();
	static int64_t tOld = t;
	const int64_t per = ticksPerSecond/2000;
	if(t > tOld + per){
		res = 1;
		tOld = t;
	}
	return res;
}

//	-------------------------------------------------------------------
//	readOuterAnalog
//	-------------------------------------------------------------------

void readOuterAnalog()
{
	if(!timeForOuterAnalog())return;

	state.axis[0].angle.raw = aiProcess(A_YAW, &angleFilt[0]);
	state.axis[1].angle.raw = aiProcess(A_PIT, &angleFilt[1]);

	float ks[NoAXES] = {kDomeEnc, kVswEnc};
	float ko[NoAXES] = {kDomeOfs, kVswOfs};
	float kc[NoAXES] = {kDomeInx, kVswInx};

	for(int i=0;i<NoAXES;i++){
		axisState_t *s = &state.axis[i];
		(s->angle.val) = gain[2][i]*((s->angle.raw) - (s->angle.ofs));
		float a = 0;
		if(i==0){
			float ac = (s->angle.val);
			ac -= kc[i];
			ac += (ac<0)?(-45):(45);
			int r = (int)(ac/90.0f);
			a = 90.0f*r;
		}
		(s->enc.ofs) = a;
		zerocount[i] = (int)(a/ks[i]);
		(s->enc.count) = getVolatile(&ecount[i]);
		(s->enc.indexed) = getVolatile(&indexed[i]);
		(s->enc.val) = (s->enc.count)*ks[i] - ko[i];
		(s->gyro.val) = (s->enc.val) - (s->angle.val);
		(s->tach.raw) = getVolatile(&tTach[i]);							// raw
		(s->tach.val) += 0.02f*((s->tach.raw) - (s->tach.val));			// filtered
	}
	static int fpass = 100;
	if(fpass){
		for(int i=0;i<NoAXES;i++){
			axisState_t *s = &state.axis[i];
			int32_t c = (int32_t)(((s->angle.val)+ko[i])/ks[i]);
			setVolatile(&ecount[i],c);
			setVolatile(&indexed[i],0);
			bump[i] = 1;
		}
		fpass--;
	}
	gyroLog();
	state.gyro.gyroFrame++;
}

//	----------------------------------------------------------------------------
//	innerPwmOut
//	----------------------------------------------------------------------------

inline void innerPwmOut(int axis, float drive, int standby)
{
	if(axis >= nMot)return;

	int pwmpin = pins[axis][0];
	int drvpin = pins[axis][1];
	int dirpin = pins[axis][2];
	int enable = pins[axis][3];

	static int olddir[3] = {0,0,0};
	axisState_t *s = &(state.axis[axis]);

	if(!enable){
		(s->pwm) = 0;
		return;
	}

	float drv = (enable<0)?(-drive):(drive);
	int pwm = (int)(drv*kpwmL);
	int dir;
	const int kPwmMin = (int)(0.08*kpwmL);

	if(standby){
		dir = 0;
		pwm = 0;
		digitalWriteFast(drvpin,0);
		digitalWriteFast(dirpin,0);
		analogWriteX(pwmpin,pwm);
	}else if(pwm < 0){
		dir = -1;
		pwm -= kPwmMin;
		pwm += kpwmL;
		pwm = max(0,min(kpwmL,pwm));
		int en = (dir==olddir[axis])?(1):(0);
		digitalWriteFast(drvpin,en);
		digitalWriteFast(dirpin,1);
		analogWriteX(pwmpin,pwm);
	}else if(pwm == 0){
		dir = 0;
		digitalWriteFast(drvpin,0);
		digitalWriteFast(dirpin,0);
		analogWriteX(pwmpin,0);		
	}else{
		dir = 1;
		pwm += kPwmMin;
		pwm = max(0,min(kpwmL,pwm));
		int en = (dir==olddir[axis])?(1):(0);
		digitalWriteFast(drvpin,en);
		digitalWriteFast(dirpin,0);
		analogWriteX(pwmpin,pwm);		
	}
	
	olddir[axis] = dir;
	(s->pwm) = pwm;
}

//	----------------------------------------------------------------------------
//	redControls
//	----------------------------------------------------------------------------

enum {
	SH_Nav_Menu_Press			= 65822,
	SH_Nav_Menu_Release			= 66078,
	SH_Nav_North_Press			= 65823,
	SH_Nav_North_Release		= 66079,
	SH_Nav_South_Press			= 65824,
	SH_Nav_South_Release		= 66080,
	SH_Nav_East_Press			= 65825,
	SH_Nav_East_Release			= 66081,
	SH_Nav_West_Press			= 65826,
	SH_Nav_West_Release			= 66082,
	SH_Nav_Enter_Press			= 65827,
	SH_Nav_Enter_Release		= 66083,
	SH_Rotary_Front_CW			= 66586,
	SH_Rotary_Front_CCW			= 67610,
	SH_Rotary_Jog_CW			= 66587,
	SH_Rotary_Jog_CCW			= 67611,
	SH_Record_Full_Press		= 65808,
	SH_Record_Full_Release		= 66064,
	BRAIN_Record_Full_Press		=   272,
	BRAIN_Record_Full_Release	=   528
};

//	----------------------------------------------------------------------------
//	sendRedControlKey
//	----------------------------------------------------------------------------

void sendRedControlKey(int key)
{
	#if defined(SerialCam)
		char s[40];
		int len = sprintf(s,"#$EXT:S:KEYCODE:%i:\n",key);
		toLog(s);
		SerialCam.write(s,len);
	#endif
}

//	----------------------------------------------------------------------------
//	sendRedShutdown
//	----------------------------------------------------------------------------

void sendRedShutdown()
{
	#if defined(SerialCam)
		char s[40];
		int len = sprintf(s,"#$EXT:S:SHUTDOWN:\n");
		toLog(s);
		SerialCam.write(s,len);
	#endif
}

//	----------------------------------------------------------------------------
//	redControls
//	----------------------------------------------------------------------------

void redControls()
{
	static steering_t oldSteering = steering;

	int ready = (steering.bits & oldSteering.bits & SB_CONS_GOOD);
	int menu = (steering.bits & SB_RED_MENU);

	if(ready && menu){

		const uint16_t b1_north = 0x40;
		const uint16_t b1_south = 0x80;
		const uint16_t b1_east  = 0x100;
		const uint16_t b1_west  = 0x200;
		const uint16_t b2_enter = 0x2;

		uint16_t b1Press =  steering.buttons1 & ~oldSteering.buttons1;
		uint16_t b2Press =  steering.buttons2 & ~oldSteering.buttons2;
		uint16_t b1Lift  = ~steering.buttons1 &  oldSteering.buttons1;
		uint16_t b2Lift  = ~steering.buttons2 &  oldSteering.buttons2;

		if(b1Press & b1_north )sendRedControlKey( SH_Nav_North_Press	);
		if(b1Lift  & b1_north )sendRedControlKey( SH_Nav_North_Release	);
		if(b1Press & b1_south )sendRedControlKey( SH_Nav_South_Press	);
		if(b1Lift  & b1_south )sendRedControlKey( SH_Nav_South_Release	);
		if(b1Press & b1_east  )sendRedControlKey( SH_Nav_East_Press		);
		if(b1Lift  & b1_east  )sendRedControlKey( SH_Nav_East_Release	);
		if(b1Press & b1_west  )sendRedControlKey( SH_Nav_West_Press		);
		if(b1Lift  & b1_west  )sendRedControlKey( SH_Nav_West_Release	);
		if(b2Press & b2_enter )sendRedControlKey( SH_Nav_Enter_Press	);
		if(b2Lift  & b2_enter )sendRedControlKey( SH_Nav_Enter_Release	);

		while(steering.enc1 > oldSteering.enc1){ sendRedControlKey(SH_Rotary_Jog_CW ); oldSteering.enc1++; }
		while(steering.enc1 < oldSteering.enc1){ sendRedControlKey(SH_Rotary_Jog_CCW); oldSteering.enc1--; }
	}

	uint16_t bPress = steering.bits & ~oldSteering.bits;

	if(bPress & SB_SHUTDOWN_RED)sendRedShutdown();
	if(steering.bits & SB_SHIFT){
		if(bPress & SB_CAM_RECORD)sendRedControlKey(BRAIN_Record_Full_Press);
		if(bPress & SB_CAM_RECORD)sendRedControlKey(BRAIN_Record_Full_Release);
	}else{
		if(bPress & SB_CAM_RECORD)sendRedControlKey(SH_Record_Full_Press);
		if(bPress & SB_CAM_RECORD)sendRedControlKey(SH_Record_Full_Release);
	}

	oldSteering = steering;
}

//	----------------------------------------------------------------------------
//  boresight
//	----------------------------------------------------------------------------

inline void boresight()
{
	boresightF();
}

//	----------------------------------------------------------------------------
//  boresightD
//	----------------------------------------------------------------------------

inline void boresightD()
{
	double dc[3][3];
	double posEarth[3];

	double aircraft_attitude[3] = {
		state.nav.Ypr[0],
		state.nav.Ypr[1],
		state.nav.Ypr[2]
	};

//	double mountSkew[3] = {0,0,0};

	double posOuter[3] = {
		state.axis[0].enc.val,
		state.axis[1].enc.val,
		state.axis[2].enc.val
	};

	double posInner[3] = {
		state.axis[0].angle.val,
		state.axis[1].angle.val,
		state.axis[2].angle.val
	};

	getDirCosinesX(&aircraft_attitude,&dc);
//	addDirCosinesX(&mountSkew,&dc);
	addDirCosinesX(&posOuter,&dc);
	addDirCosinesX(&posInner,&dc);
	rotToYprEulerX(&dc,&posEarth);

	for(int i=0;i<3;i++){
		state.geo.ypr[i] = (float)posEarth[i];
	}
}

//	----------------------------------------------------------------------------
//  boresightF
//	----------------------------------------------------------------------------

void boresightF()
{
	float dc[3][3];
	float posEarth[3];

	float aircraft_attitude[3] = {
		state.nav.Ypr[0],
		state.nav.Ypr[1],
		state.nav.Ypr[2]
	};

	float posOuter[3] = {
		state.axis[0].enc.val,
		state.axis[1].enc.val,
		state.axis[2].enc.val
	};

	float posInner[3] = {
		state.axis[0].angle.val,
		state.axis[1].angle.val,
		state.axis[2].angle.val
	};

	getDirCosinesF(&aircraft_attitude,&dc);
//	addDirCosinesF(&mountSkew,&dc);
	addDirCosinesF(&posOuter,&dc);
	addDirCosinesF(&posInner,&dc);
	rotToYprEulerF(&dc,&posEarth);

	for(int i=0;i<3;i++){
		state.geo.ypr[i] = posEarth[i];
	}
}

//  ------------------
//  hsFilterStateF
//  ------------------

typedef struct {
	float x0Gain;
	float x1Gain;
	float x2Gain;
	float y1Gain;
	float y2Gain;
	float x1;
	float x2;
	float y1;
	float y2;
} filterStateF_t;

//	----------------------------------------------------------------------------
//  analogFilter
//	----------------------------------------------------------------------------

float analogFilter(int axis,float in)
{

	static filterPar_t filtP = {FILTER_LOWPASS,40,0.707f};
    static filterStateF_t filtState[NiAXES];

	static int fpass = 1;
	float input = in;
	float output;

	if(fpass){

		fpass = 0;

		double Fs = kFreq;

		double w0;
		double alpha;
		double b0,b1,b2,a0,a1,a2;

		w0 = 2.0*PI*filtP.f1/Fs;
		alpha = sin(w0)/(2.0*filtP.q);

		//  LPF:        H(s) = 1 / (s^2 + s/Q + 1)

		b0 =  (1 - cos(w0))/2;
		b1 =   1 - cos(w0);
		b2 =  (1 - cos(w0))/2;
		a0 =   1 + alpha;
		a1 =  -2*cos(w0);
		a2 =   1 - alpha;

		if(a0!=0){
			for(int i=0;i<3;i++){
				filtState[i].x0Gain = (float)(b0/a0);
				filtState[i].x1Gain = (float)(b1/a0);
				filtState[i].x2Gain = (float)(b2/a0);
				filtState[i].y1Gain = (float)(a1/a0);
				filtState[i].y2Gain = (float)(a2/a0);
			}
		}
	}
    output =  (filtState[axis].x0Gain)*input;
    output += (filtState[axis].x1Gain)*(filtState[axis].x1);
    output += (filtState[axis].x2Gain)*(filtState[axis].x2);
    output -= (filtState[axis].y1Gain)*(filtState[axis].y1);
    output -= (filtState[axis].y2Gain)*(filtState[axis].y2);
    filtState[axis].y2 = filtState[axis].y1;
    filtState[axis].y1 = output;
    filtState[axis].x2 = filtState[axis].x1;
    filtState[axis].x1 = input;

	return output;
}

//	----------------------------------------------------------------------------
//  losFilter
//	----------------------------------------------------------------------------

float losFilter(int axis,float in)
{
	return in;

	static filterPar_t filtP = {0,5,1};
    static filterState_t filtState[NiAXES];

	static int fpass = 1;
	double input = in;
	double output;

	if(fpass){

		fpass = 0;

		double Fs = kFreq;

		double w0;
		double alpha;
		double b0,b1,b2,a0,a1,a2;

		w0 = 2.0*PI*filtP.f1/Fs;
		alpha = sin(w0)/(2.0*filtP.q);

		//  LPF:        H(s) = 1 / (s^2 + s/Q + 1)

		b0 =  (1 - cos(w0))/2;
		b1 =   1 - cos(w0);
		b2 =  (1 - cos(w0))/2;
		a0 =   1 + alpha;
		a1 =  -2*cos(w0);
		a2 =   1 - alpha;

		if(a0!=0){
			for(int i=0;i<3;i++){
				filtState[i].x0Gain = (b0/a0);
				filtState[i].x1Gain = (b1/a0);
				filtState[i].x2Gain = (b2/a0);
				filtState[i].y1Gain = (a1/a0);
				filtState[i].y2Gain = (a2/a0);
			}
		}
	}
    output =  (filtState[axis].x0Gain)*input;
    output += (filtState[axis].x1Gain)*(filtState[axis].x1);
    output += (filtState[axis].x2Gain)*(filtState[axis].x2);
    output -= (filtState[axis].y1Gain)*(filtState[axis].y1);
    output -= (filtState[axis].y2Gain)*(filtState[axis].y2);
    filtState[axis].y2 = filtState[axis].y1;
    filtState[axis].y1 = output;
    filtState[axis].x2 = filtState[axis].x1;
    filtState[axis].x1 = input;

	return (float)output;
}

//	----------------------------------------------------------------------------
//  geoFilter
//	----------------------------------------------------------------------------

float geoFilter(int axis,float in)
{
	static filterPar_t filtP = {0,3,1};
    static filterStateF_t filtState[NiAXES];

	static int fpass = 1;
	float input = in;
	float output;

	if(fpass){

		fpass = 0;

		const float Fs = 200;
		const float pi = (float)PI;

		float w0;
		float alpha;
		float b0,b1,b2,a0,a1,a2;

		w0 = 2.0f*pi*filtP.f1/Fs;
		alpha = sin(w0)/(2.0f*filtP.q);

		//  LPF:        H(s) = 1 / (s^2 + s/Q + 1)

		b0 =  (1 - cos(w0))/2;
		b1 =   1 - cos(w0);
		b2 =  (1 - cos(w0))/2;
		a0 =   1 + alpha;
		a1 =  -2*cos(w0);
		a2 =   1 - alpha;

		if(a0!=0){
			for(int i=0;i<3;i++){
				filtState[i].x0Gain = (b0/a0);
				filtState[i].x1Gain = (b1/a0);
				filtState[i].x2Gain = (b2/a0);
				filtState[i].y1Gain = (a1/a0);
				filtState[i].y2Gain = (a2/a0);
			}
		}
	}

	int geomode = (controls.mode == MODE_GEO);
	static int oldgeomode = geomode;
	if(geomode != oldgeomode){
		oldgeomode = geomode;
			for(int i=0;i<3;i++){
				filtState[i].x1 = 0;
				filtState[i].x2 = 0;
				filtState[i].y1 = 0;
				filtState[i].y2 = 0;
			}	
	}

    output =  (filtState[axis].x0Gain)*input;
    output += (filtState[axis].x1Gain)*(filtState[axis].x1);
    output += (filtState[axis].x2Gain)*(filtState[axis].x2);
    output -= (filtState[axis].y1Gain)*(filtState[axis].y1);
    output -= (filtState[axis].y2Gain)*(filtState[axis].y2);
    filtState[axis].y2 = filtState[axis].y1;
    filtState[axis].y1 = output;
    filtState[axis].x2 = filtState[axis].x1;
    filtState[axis].x1 = input;

	return output;
}

//	----------------------------------------------------------------------------
//  innerTachFilter
//	----------------------------------------------------------------------------

float innerTachFilter(int axis,float in)
{
	static filterPar_t filtP = {0,20,1};
    static filterState_t filtState[NiAXES];

	static int fpass = 1;
	double input = in;
	double output;

	if(fpass){

		fpass = 0;

		double Fs = kFreq;

		double w0;
		double alpha;
		double b0,b1,b2,a0,a1,a2;

		w0 = 2.0*PI*filtP.f1/Fs;
		alpha = sin(w0)/(2.0*filtP.q);

		//  LPF:        H(s) = 1 / (s^2 + s/Q + 1)

		b0 =  (1 - cos(w0))/2;
		b1 =   1 - cos(w0);
		b2 =  (1 - cos(w0))/2;
		a0 =   1 + alpha;
		a1 =  -2*cos(w0);
		a2 =   1 - alpha;

		if(a0!=0){
			for(int i=0;i<3;i++){
				filtState[i].x0Gain = (b0/a0);
				filtState[i].x1Gain = (b1/a0);
				filtState[i].x2Gain = (b2/a0);
				filtState[i].y1Gain = (a1/a0);
				filtState[i].y2Gain = (a2/a0);
			}
		}
	}
    output =  (filtState[axis].x0Gain)*input;
    output += (filtState[axis].x1Gain)*(filtState[axis].x1);
    output += (filtState[axis].x2Gain)*(filtState[axis].x2);
    output -= (filtState[axis].y1Gain)*(filtState[axis].y1);
    output -= (filtState[axis].y2Gain)*(filtState[axis].y2);
    filtState[axis].y2 = filtState[axis].y1;
    filtState[axis].y1 = output;
    filtState[axis].x2 = filtState[axis].x1;
    filtState[axis].x1 = input;

	return (float)output;
}

//	----------------------------------------------------------------------------
//  rangeFromPit
//	----------------------------------------------------------------------------

int rangeFromPit(float *range)
{
	const float minRange = 20;
	const float minPit = 0.1f;
	const float minH = 0.2f;

	float vAlt = (float)state.geo.cameraL[2];
	float tAlt = (float)state.geo.targetL[2];
	float dH = tAlt - vAlt;
	float pit = state.geo.ypr[1];

	if( fabs(dH) < minH )return 0;
	if( fabs(pit) < minPit )return 0;

	*range = dH/tanD(pit);
	return 1;
}

//	----------------------------------------------------------------------------
//	latLonToMeters
//	----------------------------------------------------------------------------

int latLonToMeters(double lat, double lon, double *xP, double *yP, double lat0, double lon0)
{
	const double Req = 6378137.0;
	const double F = 0.0033528107;
	static double oldLat0 = 0;
	static double cosLat;
	static double sinLat;
	static double sin2Lat;
	static double rNS;
	static double rEW;

	if(lat0 != oldLat0){
		oldLat0 = lat0;
		cosLat = cos(lat0*(PI/180.0));
		sinLat = sin(lat0*(PI/180.0));
		sin2Lat = sinLat * sinLat;
		rNS = Req * (1 + F * (3.0 * sin2Lat - 2));
		rEW = Req * (1 + F * sin2Lat);
	}
	if(abs(lon-lon0)>5)return 0;
	if(abs(lat-lat0)>5)return 0;
	*xP = (float)(rEW*(lon-lon0)*cosLat*(PI/180.0));
	*yP = (float)(rNS*(lat-lat0)*(PI/180.0));
	return 1;
}

//	----------------------------------------------------------------------------
//	latLonToMeters
//	----------------------------------------------------------------------------

int latLonToMeters(double lla[3], double *xP, double *yP, double lat0, double lon0)
{
	return latLonToMeters(lla[0],lla[1], xP, yP, lat0, lon0);
}

//	----------------------------------------------------------------------------
//	latLonToMeters
//	----------------------------------------------------------------------------

int latLonToMeters(double lla[3], double *xP, double *yP, double lla0[3])
{
	return latLonToMeters( lla[0], lla[1], xP, yP, lla0[0], lla0[1]);
}

//	----------------------------------------------------------------------------
//	latLonToNeu
//	----------------------------------------------------------------------------

int latLonToNeu(double lla[3], double (*neu)[3], double lla0[3])
{
	double n;
	double e;
	double u = lla[2]-lla0[2];
	int res = latLonToMeters( lla[0], lla[1], &e, &n, lla0[0], lla0[1]);
	if(res){
		(*neu)[0] = n;
		(*neu)[1] = e;
		(*neu)[2] = u;
	}
	return res;
}

//	----------------------------------------------------------------------------
//	metersToLatlon
//	----------------------------------------------------------------------------

void metersToLatlon(double x,double y, double *latP, double *lonP, double lat0, double lon0)
{
	static double oldLat0 = 0;
	static double kx = 0;
	static double ky = 0;

	if(lat0 != oldLat0){
		const float Req = 6378137.0f;
		const float F = 0.0033528107f;
		oldLat0 = lat0;
		float lat0F = toRadF*((float)(lat0));
		float cosLat = cosf(lat0F);
		float sinLat = sinf(lat0F);
		float sin2Lat = sinLat * sinLat;
		float rNS = Req * (1 + F * (3.0f * sin2Lat - 2));
		float rEW = Req * (1 + F * sin2Lat);
		kx = toDeg/(rEW*cosLat);
		ky = toDeg/rNS;
	}
	*lonP = lon0 + kx*x;
	*latP = lat0  + ky*y;
}

//	----------------------------------------------------------------------------
//  passive
//	----------------------------------------------------------------------------

void passive()
{
//	void passiveNew();
//	return passiveNew();

	float range = 0;
	float xyRange;
	int res = rangeFromPit(&xyRange);

	if(res){

		float yaw = toRadF*state.geo.ypr[0];

		float x = xyRange*sinf(yaw);
		float y = xyRange*cosf(yaw);

		double lat0 = state.geo.cameraL[0];
		double lon0 = state.geo.cameraL[1];
		double lat;
		double lon;
		metersToLatlon(x,y,&lat,&lon,lat0,lon0);
	
		double targetAlt = state.geo.targetL[2];

		state.geo.passiveL[0] = lat;
		state.geo.passiveL[1] = lon;
		state.geo.passiveL[2] = targetAlt;
	
		double vehAlt = state.geo.cameraL[2];
		float z = (float)(targetAlt - vehAlt);
		range = sqrtf(x*x + y*y + z*z);
	}

	state.geo.rangeP = range;
}

//	----------------------------------------------------------------------------
//  passiveRate
//	----------------------------------------------------------------------------

void passiveRatexxx()
{

	double neu0[3];
	int res = latLonToNeu( state.geo.passiveL, &neu0, state.geo.cameraL );
//	double c0[3][3];
	double c[3][3];
	yprEulerToRot(&state.geo.ypr,&c);
//	transpose3by3(&c0,&c);

	double zyx0[3];
	mult3by3times3by1(&c,&neu0,&zyx0);
	
	double dT = 0.001;
	double neu1[3] = {
		neu0[0] +  dT * state.nav.VelNed[0],
		neu0[1] +  dT * state.nav.VelNed[1],
		neu0[2] -  dT * state.nav.VelNed[2]
	};
	double zyx1[3];
	mult3by3times3by1(&c,&neu1,&zyx1);

	double x = zyx1[2];
	double dy = zyx1[1]-zyx0[1];
	double dz = zyx1[0]-zyx0[0];
	double sindy = dy/x;
	double sindz = dz/x;
	double dYaw = (abs(sindy)<=1.0)?(asin(sindy)):(0);
	double dPit = (abs(sindz)<=1.0)?(asin(sindz)):(0);

#if 1
	double k = toDeg/dT;
	state.geo.passiveRate[0] = (float)(k*dYaw);
	state.geo.passiveRate[1] = (float)(k*dPit);
	state.geo.passiveRate[2] = 0;
#elif 0
	state.geo.passiveRate[0] = (float)zyx0[0];
	state.geo.passiveRate[1] = (float)zyx0[1];
	state.geo.passiveRate[2] = (float)zyx0[2];
#else
	state.geo.passiveRate[0] = (float)neu0[0];
	state.geo.passiveRate[1] = (float)neu0[1];
	state.geo.passiveRate[2] = (float)neu0[2];
#endif

}

/*
	c[0][0] =  cosPsi
	c[0][1] =  sinPsi
	c[0][2] =  0
	c[1][0] = -sinPsi
	c[1][1] =  cosPsi
	c[1][2] =  0
	c[2][0] =  0
	c[2][1] =  0
	c[2][2] =  1

	c[0][0] =  cosTheta * cosPsi
	c[0][1] =  cosTheta * sinPsi
	c[0][2] = -sinTheta
	c[1][0] = -sinPsi
	c[1][1] =  cosPsi
	c[1][2] =  0
	c[2][0] =  sinTheta * cosPsi
	c[2][1] =  sinTheta * sinPsi
	c[2][2] =  cosTheta

	c[0][0] = cosTheta * cosPsi
	c[0][1] = cosTheta * sinPsi
	c[0][2] = -sinTheta
	c[1][0] = sinPhi * sinTheta * cosPsi - cosPhi * sinPsi
	c[1][1] = sinPhi * sinTheta * sinPsi + cosPhi * cosPsi
	c[1][2] = sinPhi * cosTheta
	c[2][0] = cosPhi * sinTheta * cosPsi + sinPhi * sinPsi
	c[2][1] = cosPhi * sinTheta * sinPsi - sinPhi * cosPsi
	c[2][2] = cosPhi * cosTheta




*/



//	---------------------
//  geo2X
//	---------------------

void geo2X()
{
	/*	double cameraE[3];
	double passiveE[3];
	double vectorE[3];
	double vectorT1[3];
	double vectorT2[3];
	double Re2t[3][3];
	double c[3][3];

	geodeticToEcef(&state.geo.cameraL, &cameraE);
	geodeticToEcef(&state.geo.passiveL, &passiveE);
	for (int i = 0; i < 3; i++) {
		vectorE[i] = passiveE[i] - cameraE[i];
	}
	earthToRe2t(&state.geo.cameraL, &Re2t);
	mult3by3times3by1(&Re2t, &vectorE, &vectorT1);

	yprEulerToRot(&state.geo.ypr, &c);

//	mult3by3times3by1(&c, &vectorT, &vectorC);
	state.geo.losError2[0] = (float)(toDeg * atan2(-vectorC[1], vectorC[0]));
	state.geo.losError2[1] = (float)(toDeg * atan2(vectorC[2], vectorC[0]));
	state.geo.losError2[2] = state.nav.Ypr[2];


	const float dT = 1.0f/200;
	static float e2Old[3]={state.geo.losError2[0],state.geo.losError2[1],state.geo.losError2[2]};
	for(int i=0;i<NiAXES;i++){
		state.geo.losErrorRate2[i] = A1minusA2(state.geo.losError2[i],e2Old[i])/dT;
		e2Old[i] = state.geo.losError2[i];
	}
	static float rangeOld = state.geo.range;
	state.geo.rangeRate = (state.geo.range-rangeOld)/dT;
	rangeOld = state.geo.range;
*/
}
/*

vn300 setup.txt

	------------------------------------
	VectorNav Control Center Baud rate
	------------------------------------

	 115200 - for VN200 in factory state
	 460800 - if Aethon setup was done

	------------------------------------
	VN-200 settings for NavBox Operation
	------------------------------------

	VNRFS*E923															// factory reset
	VNWRG,06,0,1*XX														// no logs UART1
	VNWRG,06,0,2*XX														// no logs UART2
	VNWRG,5,460800,1*020E												// UART1 460800 baud
	VNWRG,5,460800,2*326D												// UART2 460800 baud
	VNWRG,75,3,4,35,10EF,0011,0100,0600*BEC9							// log Common,UART1+UART2,800Hz/4,LogEnable,VarEnableMasks
	$VNWRG,57,+0.000,-0.3048,+0.000*XXXX								// antenna 1 offset
	$VNWRG,93,+0.000,+0.6096,+0.000,+0.0254,+0.0254,+0.0254*XXXX		// antenna 2 offset and uncertainty
	$VNWRG,26,-0.9702957,0.2419219,0.0000000,-0.2419219,-0.9702957,0.0000000,0.0000000,0.0000000,1.0000000*XXXX	// IMU orientation
	VNWNV*XX															// save

	------------------------------------
	Verification
	------------------------------------

	"Configuration" tab -> "Binary Outputs" -> Binary Output Configuration Window
	the first column should show 200Hz, 124 Bytes

*/

	geoMisc_t geoMiscP;

//	----------------------------------------------------------------------------
//  passiveRate
//	----------------------------------------------------------------------------

void passiveRate()
{
	int pr = 0;

	geodeticToEcef(&state.geo.cameraL,&geoMiscP.cameraE);
	geodeticToEcef(&state.geo.passiveL,&geoMiscP.targetE);
	for(int i=0;i<3;i++){
		geoMiscP.vectorE[i] = geoMiscP.targetE[i] - geoMiscP.cameraE[i];
	}

	earthToRe2t(&state.geo.cameraL,&geoMiscP.Re2t);
	double vecTtemp[3];
	mult3by3times3by1(&geoMiscP.Re2t,&geoMiscP.vectorE,&vecTtemp);

	// Compute ELL to Inner Axis Frame DCM
	double c[3][3];
	yprEulerToRot(&state.geo.ypr, &c);
	// compute target position wrt to the inner/imu frame
	double vectorI[3];
	mult3by3times3by1(&c, &vecTtemp, &vectorI);
	if(pr)printf("Target position in IMU/Inner frame N= %f E= %f D= %f m\n", vectorI[0], vectorI[1], vectorI[2]);
	// compute target vel in imu frame
	double velI[3];
	double camV[3];

	for(int i =0; i< 3; i++) {	
		camV[i] = state.nav.VelNed[i];
	}
	mult3by3times3by1(&c, &camV, &velI); 
	if(pr)printf("Target vel in imu frame Nvel= %f Evel= %f Dvel= %f m\n", velI[0], velI[1], velI[2]);
	float X = (float)vectorI[0];
	float Y = (float)vectorI[1];
	float Z = (float)vectorI[2];
	float Xv = (float)velI[0];
	float Yv = (float)velI[1];
	float Zv = (float)velI[2];
	float range = sqrt(Z*Z+Y*Y+X*X);
	float rangev = (X*Xv+Y*Yv+Z*Zv)/range;

	state.geo.passiveRate[0] = -(float)(toDeg*(Yv*X-Y*Xv)/(X*X+Y*Y));
	state.geo.passiveRate[1] = -(float)(toDeg*(rangev*Z-range*Zv) / (range*sqrt(range*range - Z*Z)));
	state.geo.passiveRate[2] = 0;
	if(pr)printf("Los Angular Rates to stay on target YawV = %f deg/s, PitchV = %f deg/s \n",state.geo.passiveRate[0], state.geo.passiveRate[1]);
}

//	---------------------
//  passiveNew
//	---------------------

void passiveNew()
{
	float range = 0;
	float xyRange;
	int res = rangeFromPit(&xyRange);

	if(res){

		double targetAlt = state.geo.targetL[2];
		double vehAlt = state.geo.cameraL[2];
		float yaw = state.geo.ypr[0];

		float x = xyRange*sinD(yaw);
		float y = xyRange*cosD(yaw);
		float z = (float)(targetAlt - vehAlt);
		range = sqrtf(x*x + y*y + z*z);

		double dtarg[3] = {y,x,z};
		double dtargecef[3];
		multT3by3times3by1(&geoMisc.Re2t, &dtarg, &dtargecef);
	
		double targetecef[3] = {
			geoMisc.cameraE[0] + dtargecef[0],
			geoMisc.cameraE[1] + dtargecef[1],
			geoMisc.cameraE[2] + dtargecef[2]
		};
		double passiveL[3];
		ecefToGeodetic(&targetecef, &passiveL);
	
		state.geo.passiveL[0] = passiveL[0];
		state.geo.passiveL[1] = passiveL[1];
		state.geo.passiveL[2] = targetAlt;
		state.geo.rangeP = range;
	}
}

//	----------------------------------------------------------------------------
//	triangulateTarget
//	----------------------------------------------------------------------------

bool triangulateTarget(
	double(*position1)[3], float(*los1D)[3],
	double(*position2)[3], float(*los2D)[3],
	double(*positiont)[3])
{
	enum { PSI, THETA, PHI };
	enum { YAW, PIT, ROL };
	enum { X, Y, Z };
	enum { N, E, D, U = D };

	double tanYaw1, tanYaw2;
	double ecef1[3], ecef2[3], diff[3];
	double point1[3], point2[3], point3[3], point4[3], pointa[3], pointb[3];
	double Re2tV[3][3];
	double target[3], targetecef[3];
	int i;
	double los1[3] = { toRad * (*los1D)[YAW],toRad * (*los1D)[PIT],toRad * (*los1D)[ROL] };
	double los2[3] = { toRad * (*los2D)[YAW],toRad * (*los2D)[PIT],toRad * (*los2D)[ROL] };

	tanYaw1 = tan(los1[YAW]);
	tanYaw2 = tan(los2[YAW]);
	if (fabs(tanYaw1 - tanYaw2) < (1.0 * toRad)) {
		return false;
	}

	geodeticToEcef(position1, &ecef1);
	geodeticToEcef(position2, &ecef2);

	earthToRe2t(position1, &Re2tV);
	for (i = 0; i < 3; i++)
		diff[i] = ecef2[i] - ecef1[i];
	point1[X] = point1[Y] = point1[Z] = 0.0;
	mult3by3times3by1(&Re2tV, &diff, &point3);

	point2[X] = cos(los1[YAW]) * cos(los1[PIT]);
	point2[Y] = sin(los1[YAW]) * cos(los1[PIT]);
	point2[Z] = -sin(los1[PIT]);

	point4[X] = point3[X] + cos(los2[YAW]) * cos(los2[PIT]);
	point4[Y] = point3[Y] + sin(los2[YAW]) * cos(los2[PIT]);
	point4[Z] = point3[Z] - sin(los2[PIT]);

	LineLineIntersect(&point1, &point2, &point3, &point4, &pointa, &pointb);

	for (i = 0; i < 3; i++)
		target[i] = (pointa[i] + pointb[i]) / 2.0;

	multT3by3times3by1(&Re2tV, &target, &targetecef);

	for (i = 0; i < 3; i++)
		targetecef[i] += ecef1[i];
	ecefToGeodetic(&targetecef, positiont);
	//	ecefToGeodetic(&ecef2,positiont);

	return true;
}
//	----------------------------------------------------------------------------
//	inverseResolverYp
//	----------------------------------------------------------------------------
#if 0

int lookdown;
float inverseResolver[3][3];

//	----------------------------------------------------------------------------
//	inverseResolverYp
//	----------------------------------------------------------------------------

void inverseResolverYp()
{
	const float lookdownStart = -60;
	const float lookdownHyst = 1;
	const float lookdownEnd = lookdownStart + lookdownHyst;

	float pit = state.axis[1].enc.val;
	if(pit < lookdownStart)lookdown = 1;
	if(pit > lookdownEnd  )lookdown = 0;
	float tanPit = tanD(pit);
	float cosPit = cosD(pit);
	float sinPit = sinD(pit);

	inverseResolver[1][2] = 0;
	inverseResolver[1][1] = 1;
	inverseResolver[1][0] = 0;

	if(lookdown) {                               
		inverseResolver[2][2] = 0;
		inverseResolver[2][1] = 0;
		inverseResolver[2][0] = 0;
		inverseResolver[0][2] = -1.0f/sinD(pit);
		inverseResolver[0][1] = 0;
		inverseResolver[0][0] = 0;
	}else{
		inverseResolver[2][2] = 1;
		inverseResolver[2][1] = 0;
		inverseResolver[2][0] = tanD(pit);
		inverseResolver[0][2] = 0;
		inverseResolver[0][1] = 0;
		inverseResolver[0][0] = cosD(pit);
	}
}
#endif

#if defined(_INNER)

//  ---------------------------------------------------------------------------------
//  proc2000
//  ---------------------------------------------------------------------------------

void proc2000()
{
	//  -----
	//  Setup
	//  -----

	scopeH(scopeProc2000);
	plotAxis = max(0,min((NiAXES-1),controls.plotAxis));

	//  ------------------
	//  Sample gyro inputs
	//  ------------------

	#if defined(_004)
		state.axis[0].gyro.raw = -(float)zi2c;
		state.axis[1].gyro.raw = -(float)yi2c;
		state.axis[2].gyro.raw = -(float)xi2c;
	#endif

	//  --------------------
	//  Sample analog inputs
	//  --------------------

	state.axis[0].angle.raw =	aiProcess(A_YAW, &angleFilt[0]);
	state.axis[1].angle.raw =	aiProcess(A_PIT, &angleFilt[1]);
	#ifdef A_ROL
		state.axis[2].angle.raw =	aiProcess(A_ROL, &angleFilt[2]);
	#endif

	for(int ia=0;ia<NiAXES;ia++){
		state.axis[ia].gyro.val		= gain[0][ia]*(state.axis[ia].gyro.raw		- state.axis[ia].gyro.ofs	);
		state.axis[ia].angle.val	= gain[2][ia]*(state.axis[ia].angle.raw		- state.axis[ia].angle.ofs	);
	}

	//  --------
	//  Log gyro
	//  --------

	gyroLog();

	//  --------
	//  autonull
	//  --------

	static int oldAutonull = 0;

	if(controls.autonull){
		if(oldAutonull){
			float k = (float)0.001;
			for(int ia=0;ia<NiAXES;ia++){
				state.axis[ia].gyro.ofs	+= k*(state.axis[ia].gyro.raw	- state.axis[ia].gyro.ofs);
				state.axis[ia].tach.ofs	+= k*(state.axis[ia].tach.raw	- state.axis[ia].tach.ofs);
			}
		}else{
			for(int ia=0;ia<NiAXES;ia++){
				state.axis[ia].gyro.ofs	= state.axis[ia].gyro.raw;
				state.axis[ia].tach.ofs	= state.axis[ia].tach.raw;
			}
			controls.autonull = 1;
		}
	}
	oldAutonull = controls.autonull;

	//  ------------------
	//  PID
	//  ------------------

	angles();
	transform1();
	for(int ia=0;ia<NiAXES;ia++)doPID1(ia);
	transform();
	for(int ia=0;ia<NiAXES;ia++)doPID2(ia);

	//  ------------------
	//  PID
	//  ------------------

	doPidOuter();

	//  ------------------
	//  jitter
	//  ------------------

	jitter();

	//  ------------------
	//  Exit
	//  ------------------

	scopeL(scopeProc2000);
}

//	----------------------------------------------------------------------------
//  killPID
//	----------------------------------------------------------------------------

void killPid()
{	
	controls.standby = 1;
	for(int ia=0;ia<NiAXES;ia++){
		doPID1(ia,1);
	}
//	doPidOuter(1);
}


#endif

//	----------------------------------------------------------------------------
//  outerPwmScale
//	----------------------------------------------------------------------------

float outerPwmScale(float x)
{
	const int nTab = 11;

	float table[nTab+1][2] = 
	{
		//   y   x
		{0.00f,	 0},
		{0.05f,	 5},
		{0.07f,	17},
		{0.10f,	34},
		{0.15f,	58},
		{0.20f,	75},
		{0.25f,	85},
		{0.30f,	90},
		{0.32f,	92},
		{0.35f,	94},
		{0.40f,	95},
		{0.40f,	10000}
	};

	int i = 0;
	float xa = fabs(x);
	float x0;
	float x1;
	for(i=0; i < nTab; i++){
		x0 = table[i  ][1];
		x1 = table[i+1][1];
		if( (xa>=x0) && (xa<=x1) )break;
	}
	float y0 = table[i  ][0];
	float y1 = table[i+1][0];
	float s = (y1 - y0)/(x1 - x0);
	float ya = (xa - x0)*s + y0;
	float y = (x < 0)?( -ya ):( ya );
	return y;
}