//-----------------------------------------------------------------------------
// Builds a rotation matrix that rotates one direction vector into another
//-----------------------------------------------------------------------------
void MatrixBuildTranslation( VMatrix& dst, float x, float y, float z )
{
MatrixSetIdentity( dst );
dst[0][3] = x;
dst[1][3] = y;
dst[2][3] = z;
}
void MatrixBuildTranslation( VMatrix& dst, const Vector &translation )
{
MatrixSetIdentity( dst );
dst[0][3] = translation[0];
dst[1][3] = translation[1];
dst[2][3] = translation[2];
}
void CBaseVSShader::SetVertexShaderMatrix4x4( int vertexReg, int matrixVar )
{
IMaterialVar* pTranslationVar = s_ppParams[matrixVar];
if (pTranslationVar)
{
s_pShaderAPI->SetVertexShaderConstant( vertexReg, &pTranslationVar->GetMatrixValue( )[0][0], 4 );
}
else
{
VMatrix matrix;
MatrixSetIdentity( matrix );
s_pShaderAPI->SetVertexShaderConstant( vertexReg, &matrix[0][0], 4 );
}
}
//-----------------------------------------------------------------------------
// Builds a rotation matrix that rotates one direction vector into another
//-----------------------------------------------------------------------------
void MatrixBuildRotation( VMatrix &dst, const Vector& initialDirection, const Vector& finalDirection )
{
float angle = DotProduct( initialDirection, finalDirection );
assert( IsFinite(angle) );
Vector axis;
// No rotation required
if (angle - 1.0 > -1e-3)
{
// parallel case
MatrixSetIdentity(dst);
return;
}
else if (angle + 1.0 < 1e-3)
{
// antiparallel case, pick any axis in the plane
// perpendicular to the final direction. Choose the direction (x,y,z)
// which has the minimum component of the final direction, use that
// as an initial guess, then subtract out the component which is
// parallel to the final direction
int idx = 0;
if (FloatMakePositive(finalDirection[1]) < FloatMakePositive(finalDirection[idx]))
idx = 1;
if (FloatMakePositive(finalDirection[2]) < FloatMakePositive(finalDirection[idx]))
idx = 2;
axis.Init( 0, 0, 0 );
axis[idx] = 1.0f;
VectorMA( axis, -DotProduct( axis, finalDirection ), finalDirection, axis );
VectorNormalize(axis);
angle = 180.0f;
}
else
{
CrossProduct( initialDirection, finalDirection, axis );
VectorNormalize( axis );
angle = acos(angle) * 180 / M_PI;
}
MatrixBuildRotationAboutAxis( dst, axis, angle );
#ifdef _DEBUG
Vector test;
Vector3DMultiply( dst, initialDirection, test );
test -= finalDirection;
assert( test.LengthSqr() < 1e-3 );
#endif
}
CLightingManager::CLightingManager() : BaseClass( "LightingManagerSystem" )
{
#if DEBUG
m_bVolatileLists = false;
#endif
m_bDrawWorldLights = true;
MatrixSetIdentity( m_matScreenToWorld );
m_vecViewOrigin.Init();
m_vecForward.Init();
m_flzNear = 0;
m_bDrawVolumetrics = false;
#if DEFCFG_USE_SSE
m_pSortDataX4 = NULL;
m_uiSortDataCount = 0;
#endif
}
END_SHADER_PARAMS
SHADER_INIT_PARAMS()
{
INIT_FLOAT_PARM( MAXDISTANCE, 100000.0 );
INIT_FLOAT_PARM( FARFADEINTERVAL, 400.0 );
INIT_FLOAT_PARM( MAXSIZE, 20.0 );
INIT_FLOAT_PARM( ENDFADESIZE, 20.0 );
INIT_FLOAT_PARM( STARTFADESIZE, 10.0 );
INIT_FLOAT_PARM( DEPTHBLENDSCALE, 50.0 );
INIT_FLOAT_PARM( OVERBRIGHTFACTOR, 1.0 );
INIT_FLOAT_PARM( ADDBASETEXTURE2, 0.0 );
INIT_FLOAT_PARM( ADDSELF, 0.0 );
INIT_FLOAT_PARM( ZOOMANIMATESEQ2, 0.0 );
INIT_FLOAT_PARM( ALPHATRAILFADE, 1. );
INIT_FLOAT_PARM( RADIUSTRAILFADE, 1. );
INIT_FLOAT_PARM( VERTEXFOGAMOUNT, 0.0f );
INIT_FLOAT_PARM( OUTLINEALPHA, 1.0 );
if ( !params[ORIENTATIONMATRIX]->IsDefined() )
{
VMatrix mat;
MatrixSetIdentity( mat );
params[ORIENTATIONMATRIX]->SetMatrixValue( mat );
}
if ( !params[CROPFACTOR]->IsDefined() )
{
params[CROPFACTOR]->SetVecValue( 1.0f, 1.0f );
}
if ( !params[DEPTHBLEND]->IsDefined() )
{
params[ DEPTHBLEND ]->SetIntValue( GetDefaultDepthFeatheringValue() );
}
if ( !g_pHardwareConfig->SupportsPixelShaders_2_b() )
{
params[ DEPTHBLEND ]->SetIntValue( 0 );
}
InitIntParam( DUALSEQUENCE, params, 0 );
InitIntParam( MAXLUMFRAMEBLEND1, params, 0 );
InitIntParam( MAXLUMFRAMEBLEND2, params, 0 );
InitIntParam( EXTRACTGREENALPHA, params, 0 );
InitIntParam( ADDOVERBLEND, params, 0 );
InitIntParam( BLENDFRAMES, params, 1 );
InitIntParam( DISTANCEALPHA, params, 0 );
InitIntParam( OUTLINE, params, 0 );
InitIntParam( SOFTEDGES, params, 0 );
InitIntParam( PERPARTICLEOUTLINE, params, 0 );
if ( !params[USEINSTANCING]->IsDefined() )
{
params[ USEINSTANCING ]->SetIntValue( IsX360() ? 1 : 0 );
}
// srgb read 360
InitIntParam( SHADERSRGBREAD360, params, 0 );
// default to being translucent since that's what we always were for historical reasons.
InitIntParam( OPAQUE, params, 0 );
InitIntParam( VERTEXCOLORLERP, params, 0 );
if ( !params[LERPCOLOR1]->IsDefined() )
{
params[LERPCOLOR1]->SetVecValue( 1.0f, 0.0f, 0.0f );
}
if ( !params[LERPCOLOR2]->IsDefined() )
{
params[LERPCOLOR2]->SetVecValue( 0.0f, 1.0f, 0.0f );
}
if ( params[OPAQUE]->GetIntValue() != 0 )
{
// none of these make sense if we have $opaque 1:
params[ADDBASETEXTURE2]->SetFloatValue( 0.0f );
params[DUALSEQUENCE]->SetIntValue( 0 );
params[SEQUENCE_BLEND_MODE]->SetIntValue( 0 );
params[MAXLUMFRAMEBLEND1]->SetIntValue( 0 );
params[MAXLUMFRAMEBLEND2]->SetIntValue( 0 );
params[EXTRACTGREENALPHA]->SetIntValue( 0 );
params[RAMPTEXTURE]->SetUndefined();
params[ZOOMANIMATESEQ2]->SetIntValue( 0 );
params[ADDOVERBLEND]->SetIntValue( 0 );
params[ADDSELF]->SetIntValue( 0 );
params[BLENDFRAMES]->SetIntValue( 0 );
params[DEPTHBLEND]->SetIntValue( 0 );
params[INVERSEDEPTHBLEND]->SetIntValue( 0 );
}
SET_FLAGS2( MATERIAL_VAR2_IS_SPRITECARD );
}
static int vmatrix_MatrixSetIdentity (lua_State *L) {
MatrixSetIdentity(luaL_checkvmatrix(L, 1));
return 0;
}
int main(void)
{
// <editor-fold defaultstate="collapsed" desc="Initialization of HW components/modules">
//*******************************************************************
// Initialization of HW components/modules
//===================================================================
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//===================================================================
BLIAsyncStart(50, 50); // Fast blinking - initialization
//===================================================================
MCMInitT(2.5, 2500); // Initialize Motor Control for PPM with setting
// Throttle to HIGH for delay interval to let ESC
// capture Throttle range
//--------------------------
ADCInit(3); // Initialize ADC to control battery
//--------------------------
RCInit(4); // Initialize Receiver interface with Priority=4
//--------------------------
I2CInit(5, 1); // Initialize I2C1 module with IPL=5 and Fscl=400 KHz
//--------------------------
UARTInitTX(6, 350); // Initialize UART1 for TX on IPL=6 at
// BaudRate = 48 => 115,200 bps - ZigBEE
//--------------------------------------
// BaudRate = 100 => 250,000 bps
// BaudRate = 200 => 500,000 bps
// BaudRate = 250 => 625,000 bps
// BaudRate = 350 => 833,333 bps - SD Logger, FTDI
// BaudRate = 500 => 1,250,000 bps
// BaudRate = 1000 => 2,500,000 bps
//*******************************************************************
// Initialize Magnetometer
//--------------------------------------------------------------
if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor
// ODR = 6 (max, 75 Hz),
// Gain = 1 (1.3 Gs)
// DLPF = 0 (no averaging)
BLIDeadStop("EM", 2);
//==================================================================
// Initialize motion sensor - No calibration at this time!!!
//------------------------------------------------------------------
if ( MPUInit(0, 3) ) // Initialize motion Sensor
// 1 kHz/(0+1) = 1000 Hz (1ms)
// DLPF=3 => Bandwidth 44 Hz (delay: 4.9 msec)
BLIDeadStop("EA", 2);
//------------------------------------------------------------------
//==================================================================
AltimeterInit(4); // Initialize Altimeter with IPL=4 (if needed)
// NOTE: All sensors areinitialized, so no more synchronous I2C
// operations are needed. Thus we may start Altimeter to give
// it more time to "warm-up"
AltimeterReset(); // NOTE: will be reset again later
//==================================================================
BLIAsyncStop(); // Initialization complete
//===================================================================
// </editor-fold>
//*******************************************************************
// Quadrocopter control variables
//-------------------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="Timing control variables">
ulong StartTS = 0; // Flight start time
ulong StepTS; // Control Loop step start time
ulong Alarm = 0; // "alarm" variable used to set
// the duration of the Control Loop
// </editor-fold>
// <editor-fold defaultstate="collapsed" desc="Variable required to process RC Feed ">
//-------------------------------------------------------------------
RCData RCFeed; // Control input from Receiver adjusted
// by control rates. It may also be
// corrected (if necessary) to account
// for "+" or "X" configuration as
// specified by MODE_CB_to_MF flag
//---------------------------------------
RCData RCSmthd; // Smoothed control input used in all
// subsequent control calculations
//---------------------------------------
RCData RCFinal; // Smoothed RC input adjusted for the
// Yaw angle to provide "Course Lock" (if
// required). Final RC input provided to
// Quarocopter Control Module
//-------------------------------------------------------------------
// We need battery Nominal voltage to adjust RC Throttle as battery
// charge depletes.
float BatNomV = ADCGetBatteryNomVoltage ();
//-------------------------------------------------------------------
Matrix Mtr_CB_To_MF; // Rotation matrix used to adjust Control
// Board orientation to Model front
//---------------------------------------
Matrix Mtr_CrsLck; // Rotation matrix used to adjust RC input
// to account for current Yaw to implement
// "Course Lock"
//---------------------------------------
if (MODE_CB_to_MF)
// Initialize Rotation Matrix for pre-set angle
MatrixYawRotation(__CB_To_MF_Angle__, &Mtr_CB_To_MF);
else
// Reset Rotation matrix to Identity
MatrixSetIdentity(&Mtr_CB_To_MF);
//-------------------------------------------------------------------
// </editor-fold>
DCMData IMU; // Orientation data from DCM
MCMData MC; // Motor control Variables
//-------------------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="ARMing RC and initializing IMU">
//==================================================================
// Wait for the Receiver ARMing: Throttle should go up and then down
//------------------------------------------------------------------
BLIAsyncStart(200, 200); // Really slow
RCArm();
BLIAsyncStop();
//==================================================================
// </editor-fold>
Re_Start:
//==================================================================
// <editor-fold defaultstate="collapsed" desc="Intialize Re-Start">
//------------------------------------------------------------------
StartTS = 0; // Reset timing of control Loop
//------------------------------------------------------------------
BLIAsyncMorse( "I", 1); // dit - dit
RCReadWhenReady (&RCFeed); // Get reading from receiver
//------------------------------------------------------------------
// Safety check: CONTROL should be UP and THROTTLE - LOW
//------------------------------------------------------------------
while (
0 == RCFeed.Control
||
RCFeed.Throttle > 0.1
)
{
RCReadWhenReady (&RCFeed); // Get next reading from receiver
}
//------------------------------------------------------------------
BLIAsyncStop ();
//==================================================================
//==================================================================
// Normalize last read RC values as they will represent the first
// input into the control algorithm, so should be adjusted accordingly
//------------------------------------------------------------------
NormalizeRCFeed(&RCFeed);
//------------------------------------------------------------------
// Reset Smothed RC data (initialize filter)
//------------------------------------------------------------------
RCDataReset(&RCSmthd);
//==================================================================
// Initialize sensors... Performed after ARMing to ensure that model
// is stable
//--------------------------------------------------------------
BLIAsyncStart(100, 50);
//------------------------------------------------------------------
// Start IMU and wait until orientation estimate stabilizes
//------------------------------------------------------------------
IMUReset();
//--------------------------------------------------------------
BLIAsyncStart(50, 100);
//------------------------------------------------------------------
// Reset Altimeter
//------------------------------------------------------------------
AltimeterReset();
//------------------------------------------------------------------
QCMReset (); // Initialize (reset) QCM variables
//------------------------------------------------------------------
// Force update of DCMData as inside the control Loop this
// call is non-blocking!
//------------------------------------------------------------------
if (IMU_OK != IMUGetUpdateWhenReady (&IMU))
// Failure...
BLIDeadStop ("A", 1);
//--------------------------------------------------------------
BLIAsyncStop();
//--------------------------------------------------------------
// </editor-fold>
//==================================================================
//*******************************************************************
// Quadrocopter Control Loop
//-------------------------------------------------------------------
BLISignalON();
while (1)
{
//============================================================
// Timing of control Loop
//============================================================
// <editor-fold defaultstate="collapsed" desc="Control Loop timing management">
// Sets the "frquency" of Control Loop
Alarm = TMRSetAlarm (10); // Set Alarm 10 msec in the future
//------------------------------------------------------------
StepTS = TMRGetTS (); // Capture TS of this step
if ( 0 == StartTS ) // First iteration of Control?
StartTS = StepTS; // Yes, capture timestamp
// </editor-fold>
//============================================================
//============================================================
// Read commands from receiver - non-blocking call!
// (we will get out of this call even if connection to the
// receiver is lost!)
// At the end processed Receiver data is stored in RCSmthd
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process Receiver feed">
if ( RCRead(&RCFeed) )
{
//------------------------------------------------------------
// Every NEW sample need to be "normalized"
//------------------------------------------------------------
NormalizeRCFeed(&RCFeed);
//------------------------------------------------------------
// Adjust orientation from "+" to "X" if needed
//------------------------------------------------------------
if (MODE_CB_to_MF)
{
// Control board front does not coincide with the model front
Vector RCInput;
Vector RCRotated;
VectorSet(RCFeed.Roll, RCFeed.Pitch, RCFeed.Yaw, &RCInput);
MatrixTimesVector(&Mtr_CB_To_MF, &RCInput, &RCRotated);
RCFeed.Roll = RCRotated.X;
RCFeed.Pitch = RCRotated.Y;
RCFeed.Yaw = RCRotated.Z; // NOTE: Yaw is not affected!
}
}
//============================================================
// Smooth RC data
//------------------------------------------------------------
// Roll, Pitch, and Yaw are smoothed with the IIR(4)
//------------------------------------------------------------
RCSmthd.Roll = (RCSmthd.Roll * 3.0 + RCFeed.Roll ) * 0.25; // 1/4 = 0.25
RCSmthd.Pitch = (RCSmthd.Pitch* 3.0 + RCFeed.Pitch) * 0.25;
if (MODE_Course_Lock)
{
// In this mode Yaw is integrated over the Control Loop
// duration (0.01 sec) and normalized to +/-Pi range
RCSmthd.Yaw = QCMRangeToPi(RCSmthd.Yaw + RCFeed.Yaw * 0.01);
}
else
{
// Without Course-Lock Yaw is treated as "direct input" and
// just smothed similar to other control variables.
RCSmthd.Yaw = (RCSmthd.Yaw * 3.0 + RCFeed.Yaw ) * 0.25;
}
//------------------------------------------------------------
// Throttle is smoothed with the IIR(4) and adjusted to
// account for actual battery voltage. This is done to
// improve "hovering" when throttle stick is not moving.
//------------------------------------------------------------
// Adjust Native (from RC) throttle to a value corresponding
float BatV = ADCGetBatteryVoltage();
float BatAdjTh = RCFeed.Throttle;
if (BatV > 2.0) // Sanity check :)
BatAdjTh = BatAdjTh * BatNomV / BatV;
//-----------------------------------------
RCSmthd.Throttle = (RCSmthd.Throttle*3.0 + BatAdjTh) * 0.25; // 1/4 = 0.25
//-----------------------------------------
RCSmthd.Control = RCFeed.Control;
// </editor-fold>
//============================================================
//============================================================
// Implement Motor Cut-Off if RC Control is LOW
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process RC Control">
if ( 0 == RCSmthd.Control )
{
// Yes, Control is reliably low!
//--------------------------------------------
// Override motor control
//--------------------------------------------
MC.F = MC.B = MC.L = MC.R = 0;
MCMSet(&MC);
//--------------------------------------------
// Flight terminated, post EOF to Data Logger
//--------------------------------------------
TMRDelay(10); // Wait for pipe to clear
UARTPostIfReady( NULL, 0);
// ... and again - to be sure!
TMRDelay(10); // Wait for pipe to clear
UARTPostIfReady( NULL, 0);
//----------------------------------
goto Re_Start;
}
// </editor-fold>
//============================================================
//============================================================
// Obtain and process battery charge status
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process Battery level">
float BatteryCharge = QCMBatteryMgmt();
if (BatteryCharge < 0.35) // BC < 35%
{
float MaxLevel = 2.0 * BatteryCharge;
if (RCSmthd.Throttle > MaxLevel)
RCSmthd.Throttle = MaxLevel;
}
// </editor-fold>
//============================================================
//============================================================
// Read current orientation from the IMU with sensors' raw
// measurements (non-blocking call) and, if necessary, perform
// adjustment to RCSmthd to obtain RCFinal
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process IMU data and generate Motor Control">
// First, unconditionally promote RCSmthd to RCFinal
RCDataCopy(&RCSmthd, &RCFinal);
//------------------------------------------------------------
if (IMU_OK == IMUGetUpdate(&IMU))
{
if (MODE_Course_Lock)
{
// We need to rotate RC input to adjust for current Yaw
// to implement "Course Lock" - RC Roll and Pitch commands
// are interpreted as being applied to the original (pre-
// takeoff) orientation of the model
//-------------------------------------------------------
Vector RCInput;
Vector RCRotated;
// Build rotation matrix to adjust for orientation discrepancy
MatrixYawRotation(-IMU.Yaw, &Mtr_CrsLck);
// Load RC input into RCInput vector
VectorSet(RCSmthd.Roll, RCSmthd.Pitch, RCSmthd.Yaw, &RCInput);
// Rotate RCInput vector
MatrixTimesVector(&Mtr_CrsLck, &RCInput, &RCRotated);
// Save rotated values
RCFinal.Roll = RCRotated.X;
RCFinal.Pitch = RCRotated.Y;
RCFinal.Yaw = RCRotated.Z;
}
// Calculate motor control based
// upon IMU data
QCMPerformStep(&RCFinal, &IMU, &MC);
}
else
{
// IMU Failed to provide update -
// set Motor Control to RC throttle
MC.F = MC.B = MC.L = MC.R = RCFinal.Throttle;
}
// </editor-fold>
//============================================================
//************************************************************
// Implement Motor Cut-Off to protect props if model is
// dangerously tilted (> 60 degrees) while RC Throttle is low
//------------------------------------------------------------
if (IMU.Incl <= 0.5 && RCFinal.Throttle <= 0.40)
{
// Override motor control
MC.F = MC.B = MC.L = MC.R = 0.0;
}
//------------------------------------------------------------
//************************************************************
// Update motor control
//************************************************************
MCMSet(&MC);
//------------------------------------------------------------
//===========================================================
// Load and post telemetry data (non-blocking call)
//-----------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="Populating Telemetry">
//-----------------------------------------
TM.TS = StepTS - StartTS;
//----------------------
TM.Roll = IMU.Roll;
TM.Pitch = IMU.Pitch;
TM.Yaw = IMU.Yaw;
TM.Inclination = IMU.Incl;
TM.Azimuth = IMU.Azimuth;
//----------------------
#ifdef _TMReport_GYRO_
#ifdef _TMReport_GYRO_Rotated
//------------------------------------------------------
// We rotate Gyro vector using "partial" DCM built using
// only Roll and Pitch angles as the actual Yaw does not
// affect Angualr Velocity by axis
//------------------------------------------------------
Vector Earth; // One of the Earth axis in Body frame
Vector Cross; // Temporary vector to hold the cross
// product
//------------------------------------------------------
// Calculate Roll and Pitch rates
//------------------------------------------------------
// Retrieve Earth Z-axis and calculate cross-product of
// Z-axis with Gyro vector
VectorCrossProduct(DCMEarthZ(&Earth), &IMU.GyroRate, &Cross);
TM.Gyro.X = Cross.X;
TM.Gyro.Y = Cross.Y;
//------------------------------------------------------
// Calculate Yaw rate
//------------------------------------------------------
// Retrieve Earth X-axis and calculate cross-product of
// X-axis with Gyro vector
VectorCrossProduct(DCMEarthX(&Earth), &IMU.GyroRate, &Cross);
TM.Gyro.Z = Cross.Z;
#else
// Just report native Gyro data
VectorCopy(&IMU.GyroRate, &TM.Gyro);
#endif
#endif
//----------------------
#ifdef _TMReport_ACC_
// Just report native Gyro data
VectorCopy(&IMU.Gravity, &TM.Acc);
#endif
//----------------------
TM.RollDer = QSD.RollDer;
TM.PitchDer = QSD.PitchDer;
TM.YawDer = QSD.YawDer;
//----------------------
TM.RC_Throttle = RCFinal.Throttle;
TM.RC_Roll = RCFinal.Roll;
TM.RC_Pitch = RCFinal.Pitch;
TM.RC_Yaw = RCFinal.Yaw;
//----------------------
#ifdef _TMReport_RCSmthd_
TM.RCS_Throttle = RCSmthd.Throttle;
TM.RCS_Roll = RCSmthd.Roll;
TM.RCS_Pitch = RCSmthd.Pitch;
TM.RCS_Yaw = RCSmthd.Yaw;
#endif
//----------------------
#ifdef _TMReport_RCFeed_
TM.RCN_Throttle = RCFeed.Throttle;
TM.RCN_Roll = RCFeed.Roll;
TM.RCN_Pitch = RCFeed.Pitch;
TM.RCN_Yaw = RCFeed.Yaw;
#endif
//----------------------
#ifdef _TMReport_Altimetry_
if ( 0 == AltimeterGetAltData( IMU.TS,
&IMU.Gravity,
&TM.AltResult) )
goto EndOfCycle; // Skip reporting on this step
#endif
//----------------------
#ifdef _TMReport_PID_
#ifdef _TMReport_PID_Details
TM.DRProp = QSD.DeltaRollProp;
TM.DRDiff = QSD.DeltaRollDiff;
TM.DRInt = QSD.DeltaRollInt;
#endif
TM.DRTot = QSD.DeltaRoll;
//-------------
#ifdef _TMReport_PID_Details
TM.DPProp = QSD.DeltaPitchProp;
TM.DPDiff = QSD.DeltaPitchDiff;
TM.DPInt = QSD.DeltaPitchInt;
#endif
TM.DPTot = QSD.DeltaPitch;
//-------------
#ifdef _TMReport_PID_Details
TM.DYProp = QSD.DeltaYawProp;
TM.DYDiff = QSD.DeltaYawDiff;
TM.DYInt = QSD.DeltaYawInt;
#endif
TM.DYTot = QSD.DeltaYaw;
#endif
//----------------------
TM.Throttle = QSD.Throttle; // Real Throttle
//----------------------
TM.Voltage = ADCGetBatteryVoltage();
// </editor-fold>
//===========================================================
UARTPostIfReady( (unsigned char *) &TM, sizeof(TM) );
//===========================================================
// Insert controlled "delay" to slow down the Control Loop
// to pre-set frequency
EndOfCycle:
TMRWaitAlarm(Alarm);
//===========================================================
}
//*******************************************************************
return 1;
}
