int _CRTAPI1
rc_main(
int argc,
char**argv
)
{
PCHAR r;
PCHAR x;
PCHAR s1, s2, s3;
int n;
PCHAR pchIncludeT;
ULONG cchIncludeMax;
int fInclude = TRUE; /* by default, search INCLUDE */
int fIncludeCurrentFirst = TRUE; /* by default, add current dir to start of includes */
int cDefine = 0;
int cUnDefine = 0;
PCHAR pszDefine[cDefineMax];
PCHAR pszUnDefine[cDefineMax];
CHAR szDrive[_MAX_DRIVE];
CHAR szDir[_MAX_DIR];
CHAR szFName[_MAX_FNAME];
CHAR szExt[_MAX_EXT];
CHAR szFullPath[_MAX_PATH];
CHAR szIncPath[_MAX_PATH];
CHAR buf[10];
CHAR *szRC;
PCHAR *ppargv;
int rcpp_argc;
/* Set up for this run of RC */
if (_setjmp(jb)) {
return Nerrors;
}
hHeap = RCInit();
if (hHeap == NULL) {
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1120), 0x01000000);
quit(Msg_Text);
}
pchInclude = pchIncludeT = MyAlloc(_MAX_PATH*2);
cchIncludeMax = _MAX_PATH*2;
szRC = argv[0];
/* process the command line switches */
while ((argc > 1) && (IsSwitchChar(*argv[1]))) {
switch (toupper(argv[1][1])) {
case '?':
case 'H':
/* print out help, and quit */
SendError("\n");
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(10001),
VER_PRODUCTVERSION_STR, VER_PRODUCTBUILD);
SendError(Msg_Text);
SendError(GET_MSG(20001));
SendError("\n");
return 0; /* can just return - nothing to cleanup, yet. */
case 'B':
if (toupper(argv[1][2]) == 'R') { /* base resource id */
unsigned long id;
if (isdigit(argv[1][3]))
argv[1] += 3;
else if (argv[1][3] == ':')
argv[1] += 4;
else {
argc--;
argv++;
if (argc <= 1)
goto BadId;
}
if (*(argv[1]) == 0)
goto BadId;
id = atoi(argv[1]);
if (id < 1 || id > 32767)
quit(GET_MSG(1210));
idBase = (WORD)id;
break;
BadId:
quit(GET_MSG(1209));
}
break;
case 'C':
/* Check for the existence of CodePage Number */
if (argv[1][2])
argv[1] += 2;
else {
argc--;
argv++;
}
/* Now argv point to first digit of CodePage */
if (!argv[1])
quit(GET_MSG(1204));
uiCodePage = atoi(argv[1]);
if (uiCodePage == 0)
quit(GET_MSG(1205));
/* Check if uiCodePage exist in registry. */
if (!IsValidCodePage (uiCodePage))
quit(GET_MSG(1206));
break;
case 'D':
/* if not attached to switch, skip to next */
if (argv[1][2])
argv[1] += 2;
else {
argc--;
argv++;
}
/* remember pointer to string */
pszDefine[cDefine++] = argv[1];
if (cDefine > cDefineMax) {
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1105), argv[1]);
quit(Msg_Text);
}
break;
case 'F':
switch (toupper(argv[1][2])) {
case 'O':
if (argv[1][3])
argv[1] += 3;
else {
argc--;
argv++;
}
if (argc > 1)
strcpy(resname, argv[1]);
else
quit(GET_MSG(1101));
break;
default:
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1103), argv[1]);
quit(Msg_Text);
}
break;
case 'I':
/* add string to directories to search */
/* note: format is <path>\0<path>\0\0 */
/* if not attached to switch, skip to next */
if (argv[1][2])
argv[1] += 2;
else {
argc--;
argv++;
}
if (!argv[1])
quit(GET_MSG(1201));
if ((strlen(argv[1]) + 1 + strlen(pchInclude)) >= cchIncludeMax) {
cchIncludeMax = strlen(pchInclude) + strlen(argv[1]) + _MAX_PATH*2;
pchIncludeT = MyAlloc(cchIncludeMax);
strcpy(pchIncludeT, pchInclude);
MyFree(pchInclude);
pchInclude = pchIncludeT;
pchIncludeT = pchInclude + strlen(pchIncludeT) + 1;
}
/* if not first switch, write over terminator with semicolon */
if (pchInclude != pchIncludeT)
pchIncludeT[-1] = ';';
/* copy the path */
while ((*pchIncludeT++ = *argv[1]++) != 0)
;
break;
case 'L':
/* if not attached to switch, skip to next */
if (argv[1][2])
argv[1] += 2;
else {
argc--;
argv++;
}
if (!argv[1])
quit(GET_MSG(1202));
if (sscanf( argv[1], "%x", &language ) != 1)
quit(GET_MSG(1203));
while (*argv[1]++ != 0)
;
break;
case 'M':
fMacRsrcs = TRUE;
goto MaybeMore;
case 'N':
fAppendNull = TRUE;
goto MaybeMore;
case 'P':
fPreprocessOnly = TRUE;
break;
case 'R':
goto MaybeMore;
case 'S':
// find out from BRAD what -S does
fAFXSymbols = TRUE;
break;
case 'U':
/* if not attached to switch, skip to next */
if (argv[1][2])
argv[1] += 2;
else {
argc--;
argv++;
}
/* remember pointer to string */
pszUnDefine[cUnDefine++] = argv[1];
if (cUnDefine > cDefineMax) {
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1104), argv[1]);
quit(Msg_Text);
}
break;
case 'V':
fVerbose = TRUE; // AFX doesn't set this
goto MaybeMore;
case 'X':
/* remember not to add INCLUDE path */
fInclude = FALSE;
// VC seems to feel the current dir s/b added first no matter what...
// If -X! is specified, don't do that.
if (argv[1][2] == '!') {
fIncludeCurrentFirst = FALSE;
argv[1]++;
}
MaybeMore: /* check to see if multiple switches, like -xrv */
if (argv[1][2]) {
argv[1][1] = '-';
argv[1]++;
continue;
}
break;
case 'Z':
/* if not attached to switch, skip to next */
if (argv[1][2])
argv[1] += 2;
else {
argc--;
argv++;
}
if (!argv[1])
quit(GET_MSG(1211));
s3 = strchr(argv[1], '/');
if (s3 == NULL)
quit(GET_MSG(1212));
*s3 = '\0';
MultiByteToWideChar(CP_ACP, MB_PRECOMPOSED, s3+1, -1, szSubstituteFontName, MAXTOKSTR);
s1 = argv[1];
do {
s2 = strchr(s1, ',');
if (s2 != NULL)
*s2 = '\0';
if (strlen(s1)) {
if (nBogusFontNames >= 16)
quit(GET_MSG(1213));
pszBogusFontNames[nBogusFontNames] = MyAlloc((strlen(s1)+1) * sizeof(WCHAR));
MultiByteToWideChar(CP_ACP, MB_PRECOMPOSED, s1, -1, pszBogusFontNames[nBogusFontNames], MAXTOKSTR);
nBogusFontNames += 1;
}
if (s2 != NULL)
*s2++ = ',';
}
while (s1 = s2);
*s3 = '/';
while (*argv[1]++ != 0)
;
break;
default:
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1106), argv[1]);
quit(Msg_Text);
}
/* get next argument or switch */
argc--;
argv++;
}
/* make sure we have at least one file name to work with */
if (argc != 2 || *argv[1] == '\0')
quit(GET_MSG(1107));
if (fVerbose) {
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(10001),
VER_PRODUCTVERSION_STR, VER_PRODUCTBUILD);
printf(Msg_Text);
printf("%s\n", GET_MSG(10002));
}
// Support Multi Code Page
// If user did NOT indicate code in command line, we have to set Default
// for NLS Conversion
if (uiCodePage == 0) {
CHAR *pchCodePageString;
/* At first, search ENVIRONMENT VALUE */
if ((pchCodePageString = getenv("RCCODEPAGE")) != NULL) {
uiCodePage = atoi(pchCodePageString);
if (uiCodePage == 0 || !IsValidCodePage(uiCodePage))
quit(GET_MSG(1207));
}
else
{
/* We use System ANSI Code page (ACP) */
uiCodePage = GetACP();
}
}
uiDefaultCodePage = uiCodePage;
if (fVerbose)
printf("Using codepage %d as default\n", uiDefaultCodePage);
/* If we have no extension, assumer .rc */
/* If .res extension, make sure we have -fo set, or error */
/* Otherwise, just assume file is .rc and output .res (or resname) */
//
// We have to be careful upper casing this, because the codepage
// of the filename might be in something other than current codepage.
//
MultiByteToWideChar(uiCodePage, MB_PRECOMPOSED, argv[1], -1, tokenbuf, MAXSTR+1);
if (CharUpperBuff(tokenbuf, wcslen(tokenbuf)) == 0)
_wcsupr(tokenbuf);
WideCharToMultiByte(uiCodePage, 0, tokenbuf, -1, argv[1], strlen(argv[1]), NULL, NULL);
_splitpath(argv[1], szDrive, szDir, szFName, szExt);
if (!(*szDir || *szDrive)) {
strcpy(szIncPath, ".;");
} else {
strcpy(szIncPath, szDrive);
strcat(szIncPath, szDir);
strcat(szIncPath, ";.;");
}
if ((strlen(szIncPath) + 1 + strlen(pchInclude)) >= cchIncludeMax) {
cchIncludeMax = strlen(pchInclude) + strlen(szIncPath) + _MAX_PATH*2;
pchIncludeT = MyAlloc(cchIncludeMax);
strcpy(pchIncludeT, pchInclude);
MyFree(pchInclude);
pchInclude = pchIncludeT;
pchIncludeT = pchInclude + strlen(pchIncludeT) + 1;
}
pchIncludeT = MyAlloc(cchIncludeMax);
if (fIncludeCurrentFirst) {
strcpy(pchIncludeT, szIncPath);
strcat(pchIncludeT, pchInclude);
} else {
strcpy(pchIncludeT, pchInclude);
strcat(pchIncludeT, ";");
strcat(pchIncludeT, szIncPath);
}
MyFree(pchInclude);
pchInclude = pchIncludeT;
pchIncludeT = pchInclude + strlen(pchIncludeT) + 1;
if (!szExt[0]) {
strcpy(szExt, ".RC");
}
else if (strcmp (szExt, ".RES") == 0) {
quit (GET_MSG(1208));
}
_makepath(inname, szDrive, szDir, szFName, szExt);
if (fPreprocessOnly) {
_makepath(szPreProcessName, NULL, NULL, szFName, ".rcpp");
}
/* Create the name of the .RES file */
if (resname[0] == 0) {
// if building a Mac resource file, we use .rsc to match mrc's output
_makepath(resname, szDrive, szDir, szFName, fMacRsrcs ? ".RSC" : ".RES");
}
/* create the temporary file names */
szTempFileName = MyAlloc(_MAX_PATH);
_fullpath(szFullPath, resname, _MAX_PATH);
_splitpath(szFullPath, szDrive, szDir, NULL, NULL);
_makepath(szTempFileName, szDrive, szDir, "RCXXXXXX", "");
_mktemp (szTempFileName);
szTempFileName2 = MyAlloc(_MAX_PATH);
_makepath(szTempFileName2, szDrive, szDir, "RDXXXXXX", "");
_mktemp (szTempFileName2);
ppargv = szRCPP;
*ppargv++ = "RCPP";
rcpp_argc = 1;
/* Open the .RES file (deleting any old versions which exist). */
if ((fhBin = fopen(resname, "w+b")) == NULL) {
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(1109), resname);
quit(Msg_Text);
}
else {
if (fMacRsrcs)
MySeek(fhBin, MACDATAOFFSET, 0);
if (fVerbose) {
SET_MSG(Msg_Text, sizeof(Msg_Text), GET_MSG(10102), resname);
printf(Msg_Text);
}
/* Set up for RCPP. This constructs the command line for it. */
*ppargv = _strdup("-CP");
rcpp_argc++ ; ppargv++;
_itoa(uiCodePage, buf, 10);
*ppargv = buf;
rcpp_argc++ ; ppargv++;
*ppargv = _strdup("-f");
rcpp_argc++ ; ppargv++;
*ppargv = _strdup(szTempFileName);
rcpp_argc++ ; ppargv++;
*ppargv = _strdup("-g");
rcpp_argc++ ; ppargv++;
if (fPreprocessOnly) {
*ppargv = _strdup(szPreProcessName);
} else {
*ppargv = _strdup(szTempFileName2);
}
rcpp_argc++ ; ppargv++;
*ppargv = _strdup("-DRC_INVOKED");
rcpp_argc++ ; ppargv++;
if (fAFXSymbols) {
*ppargv = _strdup("-DAPSTUDIO_INVOKED");
rcpp_argc++ ; ppargv++;
}
if (fMacRsrcs) {
*ppargv = _strdup("-D_MAC");
rcpp_argc++ ; ppargv++;
}
*ppargv = _strdup("-D_WIN32"); /* to be compatible with C9/VC++ */
rcpp_argc++ ; ppargv++;
*ppargv = _strdup("-pc\\:/");
rcpp_argc++ ; ppargv++;
*ppargv = _strdup("-E");
rcpp_argc++ ; ppargv++;
/* Parse the INCLUDE environment variable */
if (fInclude) {
*ppargv = _strdup("-I.");
rcpp_argc++ ; ppargv++;
/* add seperator if any -I switches */
if (pchInclude != pchIncludeT)
pchIncludeT[-1] = ';';
/* read 'em */
x = getenv("INCLUDE");
if (x == (PCHAR)NULL)
*pchIncludeT = '\000';
else {
if (strlen(pchInclude) + strlen(x) + 1 >= cchIncludeMax) {
cchIncludeMax = strlen(pchInclude) + strlen(x) + _MAX_PATH*2;
pchIncludeT = MyAlloc(cchIncludeMax);
strcpy(pchIncludeT, pchInclude);
MyFree(pchInclude);
pchInclude = pchIncludeT;
}
strcat(pchInclude, x);
pchIncludeT = pchInclude + strlen(pchInclude);
}
}
/* now put includes on the RCPP command line */
for (x = pchInclude ; *x ; ) {
r = x;
while (*x && *x != ';')
x = CharNextA(x);
/* mark if semicolon */
if (*x)
*x-- = 0;
if (*r != '\0' && /* empty include path? */
*r != '%' /* check for un-expanded stuff */
// && strchr(r, ' ') == NULL /* check for whitespace */
) {
/* add switch */
*ppargv = _strdup("-I");
rcpp_argc++ ; ppargv++;
*ppargv = _strdup(r);
rcpp_argc++ ; ppargv++;
}
/* was semicolon, need to fix for searchenv() */
if (*x) {
*++x = ';';
x++;
}
}
/* include defines */
for (n = 0; n < cDefine; n++) {
*ppargv = _strdup("-D");
rcpp_argc++ ; ppargv++;
*ppargv = pszDefine[n];
rcpp_argc++ ; ppargv++;
}
/* include undefine */
for (n = 0; n < cUnDefine; n++) {
*ppargv = _strdup("-U");
rcpp_argc++ ; ppargv++;
*ppargv = pszUnDefine[n];
rcpp_argc++ ; ppargv++;
}
if (rcpp_argc > MAX_CMD) {
quit(GET_MSG(1102));
}
if (fVerbose) {
/* echo the preprocessor command */
printf("RC:");
for (n = 0 ; n < rcpp_argc ; n++) {
wsprintfA(Msg_Text, " %s", szRCPP[n]);
printf(Msg_Text);
}
printf("\n");
}
/* Add .rc with rcincludes into szTempFileName */
if (!RC_PreProcess(inname))
quit(Msg_Text);
/* Run the Preprocessor. */
if (RCPP(rcpp_argc, szRCPP, NULL) != 0)
quit(GET_MSG(1116));
}
if (fPreprocessOnly) {
sprintf(szBuf, "Preprocessed File Created in: %s\n", szPreProcessName);
quit(szBuf);
}
if (fVerbose)
printf("\n%s", inname);
if ((fhInput = fopen(szTempFileName2, "rb")) == NULL_FILE)
quit(GET_MSG(2180));
if (!InitSymbolInfo())
quit(GET_MSG(22103));
LexInit (fhInput);
uiCodePage = uiDefaultCodePage;
ReadRF(); /* create .RES from .RC */
if (!TermSymbolInfo(fhBin))
quit(GET_MSG(22204));
if (!fMacRsrcs)
MyAlign(fhBin); // Pad end of file so that we can concatenate files
CleanUpFiles();
HeapDestroy(hHeap);
return Nerrors; // return success, not quitting.
}
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
//--------------------------
BLIAsyncMorse("S", 1); // dit-dit-dit
MCMInitT(2.5, 2500); // Initialize Motor Control for PPM with setting
// Throttle to HIGH for delay interval to let ESC
// capture Throttle range
BLIAsyncMorse("O", 1); // dah-dah-dah
//--------------------------
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
// BaudRate = 500 => 1,250,000 bps
// BaudRate = 1000 => 2,500,000 bps
//*******************************************************************
// <editor-fold defaultstate="collapsed" desc="Initializing IMU">
//==================================================================
#ifdef __MAG_Use__
//--------------------------------------------------------------
// Initialize Magnetometer
//--------------------------------------------------------------
if (HMCInit(6, 1, 0)) // Initialize magnetic Sensor
// ODR = 6 (max, 75 Hz),
// Gain = 2 (1.3 Gs)
// DLPF = 0 (no averaging)
BLIDeadStop("EM", 2);
#endif
//*******************************************************************
BLIAsyncMorse( "I", 1); // dit - dit
//==================================================================
#ifdef __MXB_Use__
//--------------------------------------------------------------
// Initialize MaxBotix range finder
//--------------------------------------------------------------
if ( 0 == MXBInit(3, &TM.MXB) )
BLIDeadStop("ES", 2);
#endif
//==================================================================
// Initialize motion sensor - rotation rate baseline established at
// this time - QUAD should be motionless!!!
//------------------------------------------------------------------
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);
//------------------------------------------------------------------
BLIAsyncStop();
//==================================================================
// </editor-fold>
//==================================================================
BLISignalON();
TMRDelay(2000); // Wait for extra 2 sec - to let ESC arm...
// (finish the song :) )
BLISignalOFF();
//===================================================================
// </editor-fold>
//*******************************************************************
// Quadrocopter control variables
//-------------------------------------------------------------------
ulong StartTS = 0;
ulong StepTS;
RCData RCNative; // Control input from Receiver
RCData RC; // Smoothed control input used in all
// control calculations
DCMData IMU; // Orientation data from DCM
MCMData MC; // Motor control Variables
float BatNomV = ADCGetBatteryNomVoltage();
ulong Alarm = 0;
#ifdef __CB_To_Model_Front__
// RC native input rotated to adjust CB orientation to model front
RCData RC_CB_To_Model_Front;
// Control board front does not coincide with the model front
Matrix CB_To_Model;
// Build rotation matrix to adjust for orientation discrepancy
MatrixYawRotation(__CB_To_Model_Front__, &CB_To_Model);
#else
// If CB orientation coincides with the fron of the model
// we will use Native RC input as base for RC input to
// Quadrocopter control module
#define RC_CB_To_Model_Front RCNative
#endif
Re_Start:
//==================================================================
// Wait for the Receiver ARMing: Control should go Down and then Up
//------------------------------------------------------------------
BLIAsyncMorse( "O", 1); // doh - doh - doh
RCSymArm();
//==================================================================
BLIAsyncMorse("T", 1); // doh
MPUAsyncStop();
if (MPUCalibrate() != MPU_OK)
// Gyro Calibration filed
BLIDeadStop("EA", 2);
BLIAsyncStop();
//==================================================================
// Start IMU and wait until orientation estimate stabilizes
//------------------------------------------------------------------
BLIAsyncMorse( "E", 1); // dit
IMUInit();
//------------------------------------------------------------------
QCMReset(); // Initialize (reset) QCM variables
//------------------------------------------------------------------
BLIAsyncStop();
//==================================================================
//******************************************************************
// Control variables used to smooth RC receiver data
//------------------------------------------------------------------
// Reset Smothed RC data
//-------------------------------------------------------------------
RC.Roll = 0.0;
RC.Pitch = 0.0;
RC.Yaw = 0.0;
//------------------------
RC.Throttle = 0.0;
//------------------------
RC.Control = 1;
//-------------------------------------------------------------------
//*******************************************************************
// Quadrocopter Control Loop
//-------------------------------------------------------------------
BLISignalON();
while (1)
{
// Sets the "frquency" of Control Loop
Alarm = TMRSetAlarm(10); // Set Alarm 10 msec in the future
//============================================================
// Read commands from receiver - non-blocking call!
// (we will get out of this call even if connection to the
// receiver is lost!)
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process Receiver feed">
if ( RCSymRead(&RCNative) )
{
//------------------------------------------------------------
// Normalize Roll and Pitch control input from RC Receiver to
// +/- 0.35 rad (~20 degrees) and Yaw control input to
// +/- 3.00 rad (~172 degrees)
//------------------------------------------------------------
RCNative.Roll = 0.35 * RCNative.Roll;
RCNative.Pitch = 0.35 * RCNative.Pitch;
RCNative.Yaw = 3.00 * RCNative.Yaw;
#ifdef __CB_To_Model_Front__
{
// Control board front does not coincide with the model front
Vector RCInput;
Vector RCRotated;
VectorSet(RCNative.Roll, RCNative.Pitch, RCNative.Yaw, &RCInput);
MatrixTimesVector(&CB_To_Model, &RCInput, &RCRotated);
RC_CB_To_Model_Front.Roll = RCRotated.X;
RC_CB_To_Model_Front.Pitch = RCRotated.Y;
RC_CB_To_Model_Front.Yaw = RCRotated.Z;
}
#endif
}
//============================================================
// Smooth RC data
//------------------------------------------------------------
// Roll, Pitch, and Yaw are smoothed with the IIR(8)
//------------------------------------------------------------
RC.Roll = (RC.Roll * 7.0 + RC_CB_To_Model_Front.Roll ) * 0.125; // 1/8 = 0.125
RC.Pitch = (RC.Pitch * 7.0 + RC_CB_To_Model_Front.Pitch) * 0.125;
RC.Yaw = (RC.Yaw * 7.0 + RC_CB_To_Model_Front.Yaw ) * 0.125;
//------------------------------------------------------------
// 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 BatAdjTh = RCNative.Throttle * BatNomV / ADCGetBatteryVoltage();
//-----------------------------------------
RC.Throttle = (RC.Throttle * 3 + BatAdjTh) * 0.25; // 1/4 = 0.25
//-----------------------------------------
RC.Control = RCNative.Control;
// </editor-fold>
//============================================================
//============================================================
// Implement Motor Cut-Off if RC Control is LOW
//============================================================
// <editor-fold defaultstate="collapsed" desc="Process RC Control">
if ( 0 == RC.Control )
{
// Yes, Control is reliably low!
//--------------------------------------------
// Override motor control
//--------------------------------------------
MC.F = MC.B = MC.L = MC.R = 0;
MCMSet(&MC);
//--------------------------------------------
// Reset Timing series
StartTS = 0;
//--------------------------------------------
// 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 (RC.Throttle > MaxLevel)
RC.Throttle = MaxLevel;
}
// </editor-fold>
//============================================================
//============================================================
// Read current orientation and sensor data from the IMU
// (non-blocking call)
//============================================================
if (IMU_OK == IMUGetUpdate(&IMU))
{
// Calculate motor control based
// upon IMU data
QCMPerformStep(&RC, &IMU, &MC);
}
else
{
// IMU Failed to provide update -
// set Motor Control to native throttle
MC.F = MC.B = MC.L = MC.R = RC.Throttle;
}
//============================================================
//*****************************************
// Implement Motor Cut-Off if model is
// dangerously tilted (> 60 degrees) while
// RC Throttle is low - to protect props
//----------------------------------------
if (IMU.Incl <= 0.5 && RC.Throttle <= 0.40)
{
// Override motor control
MC.F = MC.B = MC.L = MC.R = 0;
}
//----------------------------------------
//*****************************************
// Update motor control
//*****************************************
MCMSet(&MC);
//-----------------------------------------
//===========================================================
// Load and post telemetry data (non-blocking call)
//-----------------------------------------------------------
// <editor-fold defaultstate="collapsed" desc="Populating Telemetry">
StepTS = TMRGetTS();
//-----------------------------------------
if ( 0 == StartTS )
// Time stamp of cycle start!
StartTS = StepTS;
//-----------------------------------------
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
//------------------------------------------------------
Matrix NoYawDCM;
// Generate partial rotation matrix
MatrixEulerRotation(IMU.Roll, IMU.Pitch, 0.0, &NoYawDCM);
// Rotate Gyro vector
MatrixTimesVector(&NoYawDCM, &IMU.GyroRate, &TM.Gyro);
#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 = RC.Throttle;
TM.RC_Roll = RC.Roll;
TM.RC_Pitch = RC.Pitch;
TM.RC_Yaw = RC.Yaw;
//----------------------
#ifdef _TMReport_NativeRC_
TM.RCN_Throttle = RCNative.Throttle;
TM.RCN_Roll = RCNative.Roll;
TM.RCN_Pitch = RCNative.Pitch;
TM.RCN_Yaw = RCNative.Yaw;
#endif
//----------------------
#ifdef __MXB_Use__
MXBRead(&TM.MXB);
#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 = QSD.Voltage;
// </editor-fold>
//===========================================================
UARTPostIfReady( (unsigned char *) &TM, sizeof(TM) );
//===========================================================
// Insert controlled "delay" to slow down the Control Loop
TMRWaitAlarm(Alarm);
}
//*******************************************************************
return 1;
}
i32 RCMain()
{
i32 bail = 0;
#ifdef WIN32
while (!SetCurrentDirectoryA(RC_RESOURCE_PATH) && SetCurrentDirectoryA("..")) {
bail++;
if (bail >= 32) {
printf("Can't find resource folder. Build directory must be located further down in the hierarchy than /res/\n");
exit(0);
}
}
SetCurrentDirectoryA("..");
#else
char *fbuf = __FILE__;
char fpath [8192];
char *epos = strrchr(fbuf, '/');
strncpy(fpath, fbuf, (epos - fbuf));
fpath[(epos - fbuf)] = 0;
chdir(fpath);
while (chdir(RC_RESOURCE_PATH) != 0 && chdir("..") == 0) {
bail++;
if (bail >= 32) {
printf("Can't find resource folder. Build directory must be located further down in the hierarchy than /res/\n");
exit(0);
}
}
chdir("..");
#endif
char buf[4096];
printf("Result setup...\n");
ResultInit();
printf("Done\n");
printf("Display setup...\n");
Platform::setDisplay(false, RC_DEFAULT_DISPLAY_WIDTH, RC_DEFAULT_DISPLAY_HEIGHT);
printf("Done\n");
printf("Renderer init...\n");
Renderer::init();
printf("Done\n");
printf("SceneGraph setup...\n");
SceneGraph::init();
printf("Done\n");
printf("Console setup...\n");
Console::init();
printf("Done\n");
printf("Command setup...\n");
Command::init();
printf("Done\n");
printf("Frame init...\n");
Platform::initFrame();
printf("Done\n");
Console::log("~c9995=============================");
Console::log("~c3939~g3939 _");
Console::log("~c3939~g6969 c \" ~r-[ RenderChimp ]-");
Console::log("~c6969~g9999 (- ~r-[ v 0.3.2a ]-\n ");
Console::log("~c9995=============================\n \n ");
//Console::log("~c9995Press ~c6966F1~c9995 to bring up the console.");
Console::log("~c9995Press F1 to bring up the console.");
sprintf(buf, "/config %sconfig/config.cfg", RC_RESOURCE_PATH);
Command::run(buf);
#if defined RC_WIN32
Console::log("Running on Win32 platform");
#else
Console::log("Running on some platform");
#endif
printf("Running RCInit...\n");
RCInit();
printf("Done\n");
Console::log("Initiated...");
rs.enableDepthTest(CMP_LESS);
rs.enableDepthWrite();
printf("Running main loop...\n");
Platform::run();
return 0;
}
int main(void)
{
//*******************************************************************
Init();
TMRInit(2); // Initialize Timer interface with Priority=2
BLIInit(); // Initialize Signal interface
//--------------------------
BLIAsyncMorse("S", 1); // dot-dot-dot
MCMInitF(50, 2500); // Initialize Motor Control at 50 Hz with setting
// Throttle to HIGH for delay interval to let ESC
// capture Throttle range
BLIAsyncStop();
//--------------------------
RCInit(4); // Initialize Receiver interface with Priority=4
//--------------------------
UARTInitTX(6, 48); // Initialize UART1 for TX on IPL=6 at 115200 bps
// This initialization routine accepts BaudRate in multiples
// of 2400 bps; Thus:
// BaudRate = 1 => 2400 bps
// BaudRate = 2 => 4800 bps
// ...
// BaudRate = 48 => 115200 bps
//------------------------------------------------------------
// High speed
//------------------------------------------------------------
// BaudRate = 100 => 250,000 bps
// BaudRate = 200 => 500,000 bps
// BaudRate = 250 => 625,000 bps
// BaudRate = 350 => 833,333 bps
// BaudRate = 500 => 1,250,000 bps
// BaudRate = 1000 => 2,500,000 bps
//*******************************************************************
BLISignalON();
TMRDelay(2000); // Wait for extra 2 sec - to let ESC arm...
// (finish the song :) )
BLISignalOFF();
//==================================================================
MCMData MC;
RCData RC;
//-------------------------------------------------
BLIAsyncMorse("R", 1); // dot-doh-dot
RCArm();
BLIAsyncStop();
//-------------------------------------------------
BLISignalON();
while(1)
{
RCReadWhenReady(&RC);
//---------------------------------------------
if (0 == RC.Control)
MC.F = MC.B = MC.L = MC.R = 0.0;
else
{
MC.F = RC.Throttle;
MC.B = RC.Throttle;
//--------------
MC.L = RC.Throttle;
MC.R = RC.Throttle;
//--------------
}
//---------------------------------------------
MCMSet(&MC);
//---------------------------------------------
UARTPostWhenReady((uchar*)&RC, sizeof(RC));
//---------------------------------------------
BLISignalFlip();
}
return 1;
}
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;
}