/* MakeBulkPacket - creates a bulkread packet based on the ping results.
* It only needs to be called once because the packet will never change.
* If the user wishes to use bulkread, this function must be called first.
*/
void DarwinController::MakeBulkPacket(unsigned char *BulkReadTxPacket){
int number = 0;
BulkReadTxPacket[0] = 0xFF;
BulkReadTxPacket[1] = 0xFF;
BulkReadTxPacket[ID] = 0xFE;
BulkReadTxPacket[INSTRUCTION] = 0x92;
BulkReadTxPacket[PARAMETER] = 0x0;
if(Ping(0xC8, 0)){
BulkReadTxPacket[PARAMETER+3*number+1] = 30; //length
BulkReadTxPacket[PARAMETER+3*number+2] = 0xC8; // ID_CM
BulkReadTxPacket[PARAMETER+3*number+3] = 24; //P_DXL_POWER
number++;
}
if(Ping(70, 0)){
BulkReadTxPacket[PARAMETER+3*number+1] = 10; // length
BulkReadTxPacket[PARAMETER+3*number+2] = 0x70; // ID_L_FSR
BulkReadTxPacket[PARAMETER+3*number+3] = 0x1A; // start address P_FSR1_L
number++;
}
if(Ping(0x6F, 0)){
BulkReadTxPacket[PARAMETER+3*number+1] = 10; // length
BulkReadTxPacket[PARAMETER+3*number+2] = 0x6F; // id ID_R_FSR
BulkReadTxPacket[PARAMETER+3*number+3] = 0x1A; // start address P_FSR1_L
number++;
}
for(int id = 1; id < 21; id++){ //NUMBER OF JOINTS = 20
BulkReadTxPacket[PARAMETER+3*number+1] = 23; // length
BulkReadTxPacket[PARAMETER+3*number+2] = id; // id
BulkReadTxPacket[PARAMETER+3*number+3] = 26; // start at CCW_COMPLIANCE_MARGIN
number++;
}
BulkReadTxPacket[LENGTH] = (number * 3) + 3;
int length = BulkReadTxPacket[LENGTH] + 4;
BulkReadTxPacket[length - 1] = CalculateChecksum(BulkReadTxPacket);
}
int
DynamixelComm::Receive(unsigned char * b)
{
int c = ReceiveBytes((char *)b, 4);
fprintf(stdout, "in receive, c = %d b = %s\n", c, b);
if(c < 4)
{
printf("receive error (data size = %d)\n", c);
return 0;
}
c += ReceiveBytes((char *)&b[4], b[3]);
unsigned char checksum = CalculateChecksum(b);
fprintf(stdout, "I dunno: %d %d\n", checksum, b[b[3] + 3]);
if(checksum != b[b[3]+3])
{
printf("receive error (data size = %d) incorrect checksum\n", c);
return -1;
}
return c;
}
/*
================
idProgram::Restore
================
*/
bool idProgram::Restore( idRestoreGame *savefile )
{
int i, num, index;
bool result = true;
idStr scriptname;
savefile->ReadInt( num );
for( i = 0; i < num; i++ )
{
savefile->ReadString( scriptname );
CompileFile( scriptname );
}
savefile->ReadInt( index );
while( index >= 0 )
{
savefile->ReadByte( variables[index] );
savefile->ReadInt( index );
}
savefile->ReadInt( num );
for( i = variableDefaults.Num(); i < num; i++ )
{
savefile->ReadByte( variables[i] );
}
int saved_checksum, checksum;
savefile->ReadInt( saved_checksum );
checksum = CalculateChecksum();
if( saved_checksum != checksum )
{
result = false;
}
return result;
}
/******************************************************
* Takes in an instruction and list of params *
* Params needs to be in the form: *
* {ID_1, data_1, ID_2, data_2, ...} *
* There can be any amount of motors and in any order *
* Note that paramlength does not include the ID *
******************************************************/
int DarwinController::SyncWrite(unsigned char* packet, unsigned char instruction, unsigned char* params, unsigned char numparams, unsigned char paramlength){
unsigned char len = numparams + 7; //Last index of array (where checksum goes)
packet[0] = 0xFF; // Heading
packet[1] = 0xFF;
packet[ID] = 0xFE; // Broadcast ID
packet[LENGTH] = len-3; // Length of packet except for headings, ID, Checksum
packet[INSTRUCTION] = 0x83; // Sync Write
packet[5] = instruction; // What is happening at each motor
packet[6] = paramlength; // Number of bytes written to each motor
for(unsigned char i = 0; i < numparams; i++){
packet[7+i] = params[i];
}
packet[len] = CalculateChecksum(packet);
unsigned char rxpacket[MAXNUM_RXPARAM + 10] = {0, };
return ReadWrite(packet, rxpacket);
//return port.WritePort(packet, len+1); /alt method
}
void GetGPSData(void)
{
union longbbbb Temp4;
union intbb Temp2;
int LocalDays = XPLMGetDatai(drLocalDays);
float LocalSecsFloat = XPLMGetDataf(drLocalSecs) * 1000;
LocalDays += 5;
int Week = (int)(LocalDays / 7) + 1564;
LocalDays = (LocalDays % 7);
Week = (Week * 10) + LocalDays;
int LocalSecsInt = (int)LocalSecsFloat + (LocalDays * 86400000);
LocalSecsFloat = (LocalSecsFloat - (int)LocalSecsFloat) * 1000000;
Temp2.BB = Week;
Store2LE(&NAV_SOL[14], Temp2);
Temp4.WW = LocalSecsInt;
Store4LE(&NAV_SOL[6], Temp4);
Store4LE(&NAV_DOP[6], Temp4);
Store4LE(&NAV_POSLLH[6], Temp4);
Store4LE(&NAV_VELNED[6], Temp4);
Temp4.WW = (int)LocalSecsFloat;
Store4LE(&NAV_SOL[10], Temp4);
double latitude = XPLMGetDataf(drLat);
double longitude = XPLMGetDataf(drLon);
double elevation = XPLMGetDataf(drElev);
double local_x = XPLMGetDataf(drLocal_x);
double local_y = XPLMGetDataf(drLocal_y);
double local_z = XPLMGetDataf(drLocal_z);
double local_vx = XPLMGetDataf(drLocal_vx);
double local_vy = XPLMGetDataf(drLocal_vy);
double local_vz = XPLMGetDataf(drLocal_vz);
Temp4.WW = (int)(local_vx * 100);
Store4LE(&NAV_VELNED[14], Temp4);
Temp4.WW = (int)(local_vy * -100);
Store4LE(&NAV_VELNED[18], Temp4);
Temp4.WW = (int)(local_vz * -100);
Store4LE(&NAV_VELNED[10], Temp4);
Temp4.WW = (int)(XPLMGetDataf(drAirSpeedTrue) * 100);
Store4LE(&NAV_VELNED[22], Temp4);
Temp4.WW = (int)(XPLMGetDataf(drGroundSpeed) * 100);
Store4LE(&NAV_VELNED[26], Temp4);
Temp4.WW = (int)(XPLMGetDataf(drHeading) * 100000);
Store4LE(&NAV_VELNED[30], Temp4);
Temp4.WW = (int)(latitude * 10000000);
Store4LE(&NAV_POSLLH[14], Temp4);
Temp4.WW = (int)(longitude * 10000000);
Store4LE(&NAV_POSLLH[10], Temp4);
Temp4.WW = (int)(elevation * 1000);
Store4LE(&NAV_POSLLH[18], Temp4);
Store4LE(&NAV_POSLLH[22], Temp4);
double ac_pos_lat, ac_pos_lon, ac_pos_elev;
double ac_vel_lat, ac_vel_lon, ac_vel_elev;
// Get AC pos in LLA
XPLMLocalToWorld( local_x,
local_y,
local_z,
&ac_pos_lat,
&ac_pos_lon,
&ac_pos_elev );
// Get AC pos + velocity vector in LLA
XPLMLocalToWorld( local_x + local_vx,
local_y + local_vy,
local_z + local_vz,
&ac_vel_lat,
&ac_vel_lon,
&ac_vel_elev );
// convert to ECEF
LLAtoECEF(ac_pos_lat, ac_pos_lon, ac_pos_elev, local_x, local_y, local_z);
LLAtoECEF(ac_vel_lat, ac_vel_lon, ac_vel_elev, local_vx, local_vy, local_vz);
// AC pos stays as is
// subtract to get velocity vector in ECEF
local_vy -= local_y;
local_vx -= local_x;
local_vz -= local_z;
Temp4.WW = (int)(local_x * 100);
Store4LE(&NAV_SOL[18], Temp4);
Temp4.WW = (int)(local_y * 100);
Store4LE(&NAV_SOL[22], Temp4);
Temp4.WW = (int)(local_z * 100);
Store4LE(&NAV_SOL[26], Temp4);
Temp4.WW = (int)(local_vx * 100);
Store4LE(&NAV_SOL[34], Temp4);
Temp4.WW = (int)(local_vy * 100);
Store4LE(&NAV_SOL[38], Temp4);
Temp4.WW = (int)(local_vz * 100);
Store4LE(&NAV_SOL[42], Temp4);
CalculateChecksum(NAV_SOL);
SendToComPort(sizeof(NAV_SOL),NAV_SOL);
CalculateChecksum(NAV_DOP);
SendToComPort(sizeof(NAV_DOP),NAV_DOP);
CalculateChecksum(NAV_POSLLH);
SendToComPort(sizeof(NAV_POSLLH),NAV_POSLLH);
CalculateChecksum(NAV_VELNED);
SendToComPort(sizeof(NAV_VELNED),NAV_VELNED);
}
float GetBodyRates(float elapsedMe, float elapsedSim, int counter, void * refcon)
{
union intbb Temp2;
float phi, theta, psi;
float alpha, beta;
float P_flight, Q_flight, R_flight;
float ax, ay, az;
// Angular rates in X-Plane are specified relative to the flight path, not to the aircraft,
// for reasons unknown. So that means we need to rotate by alpha and beta to get angular rates
// in the aircraft body frame, which is what the UDB measures.
// Retrieve rates and slip angles, and convert to radians
P_flight = XPLMGetDataf(drP) / 180 * PI ;
Q_flight = XPLMGetDataf(drQ) / 180 * PI ;
R_flight = XPLMGetDataf(drR) / 180 * PI ;
alpha = XPLMGetDataf(drAlpha) / 180 * PI;
beta = XPLMGetDataf(drBeta) / 180 * PI;
FLIGHTtoBCBF(P_flight, Q_flight, R_flight, alpha, beta);
P_plane = P_flight;
Q_plane = Q_flight;
R_plane = R_flight;
// Angular rate
// multiply by 5632 (constant from UDB code)
// Divide by SCALEGYRO(3.0 for red board)
// 1 * 5632 / 3.0 = 1877.33
Temp2.BB = (int)(P_plane * 1877.33);
Store2LE(&NAV_BODYRATES[6], Temp2);
Temp2.BB = (int)(Q_plane * 1877.33);
Store2LE(&NAV_BODYRATES[8], Temp2);
Temp2.BB = (int)(R_plane * 1877.33);
Store2LE(&NAV_BODYRATES[10], Temp2);
// Our euler angles:
// X-Plane angles are in degrees.
// Phi is roll, roll to right is positive
// Theta is pitch, pitch up is positive
// Psi is yaw, relative to north. North is 0/360.
// Convert these angles to radians first.
phi =(XPLMGetDataf(drPhi)) / 180 * PI * -1.0;
theta = (XPLMGetDataf(drTheta)) / 180 * PI;
psi = (XPLMGetDataf(drPsi)) / 180 * PI * -1.0;
// Get accelerations in OpenGL coordinate frame
//ax = XPLMGetDataf(drLocal_ax);
//ay = XPLMGetDataf(drLocal_ay);
//az = XPLMGetDataf(drLocal_ay);
ax = 0;
ay = 0;
az = 0;
// Gravity is not included in ay, we need to add it. OGL y axis is +ve up,
// so g is -9.8.
ay -= (float)9.8;
// Convert from OGL frame to Aircraft body fixed frame
OGLtoBCBF(ax, ay, az, phi, theta, psi);
ax_plane = ax;
ay_plane = ay;
az_plane = az;
// Lastly we need to convert from X-Plane units (m/s^2) to the arbitrary units used by the UDB
// Accelerations are in m/s^2
// Divide by 9.8 to get g's
// Multiply by 5280 (constant from UDB code)
// Divide by SCALEACCEL (2.64 for red board)
// 1 / 9.8 * 5280 / 2.64 = 204.8
Temp2.BB = (int)(ax * 204.8);
Store2LE(&NAV_BODYRATES[12], Temp2);
Temp2.BB = (int)(ay * 204.8);
Store2LE(&NAV_BODYRATES[14], Temp2);
Temp2.BB = (int)(az * 204.8);
Store2LE(&NAV_BODYRATES[16], Temp2);
CalculateChecksum(NAV_BODYRATES);
SendToComPort(sizeof(NAV_BODYRATES),NAV_BODYRATES);
ReceiveFromComPort();
ServosToControls();
// float ThrottleSetting = 0; //SurfaceDeflections[CHANNEL_THROTTLE];
// float throttle[8] = {ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting,
// ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting};
XPLMSetDatavf(drThro, ThrottleSettings,0,8);
return -1;
}
void PollingWorker::PollRS232()
{
char * readBuf;
char * rawByte;
char * unCompressed;
Header * headerBuffer;
Item * sender;
long numBytesToGet;
char receiveID;
DWORD dwCommEvent, dwBytesTransferred;
Msg * newMsg;
isFinish = 0;
while(!isFinish)
{
SetUpDCB(baudRate);
// set up the mask, EV_RXCHAR is the event when we receive a character
if (!SetCommMask(hComm, EV_RXCHAR))
emit error(QString("Error setting communications mask."), (int)GetLastError());
// wait for a character to come in
if (!WaitCommEvent(hComm, &dwCommEvent, NULL))
emit error(QString("Error waiting for a character."), (int)GetLastError());
// we have a character, read the header to see if its good
else
{
if(!isRaw->isChecked())
{
// set up the header buffer
if(!(headerBuffer = (Header *)malloc(sizeof(struct Header))))
emit error(QString("Error malloccing headerBuffer."), (int)GetLastError());
// get the header
if(!ReadFile(hComm, (BYTE *)headerBuffer, HEADERSIZE, &dwBytesTransferred, 0))
emit error(QString("Error getting the header buffer."), (int)GetLastError());
if(headerBuffer->lSignature == 0xDEADBEEF)
{
// get the data length from the header
numBytesToGet = headerBuffer->lDataLength;
readBuf = (char*)calloc(numBytesToGet,sizeof(char));
if (readBuf == NULL)
emit error(QString("Error mallocing readBuf."), (int)GetLastError());
// get the message
if(!ReadFile(hComm, readBuf, numBytesToGet, &dwBytesTransferred, 0))
emit error(QString("Error getting the message."), (int)GetLastError());
emit error(QString("Bytes gotten"), (int)(dwBytesTransferred));
unCompressed = readBuf;
// calculate the checksum and compare
if(headerBuffer->sChecksum != CalculateChecksum(readBuf, headerBuffer->lDataLength))
{
emit transmitError();
//emit error (QString ("Checksum reports errors"),0);
}
if (headerBuffer->bVersion == 0xFF)
{
if(!(unCompressed = (char *)calloc(headerBuffer->lDataUncompressed,sizeof(char))))
emit error(QString("Error malloccing unCompressed."), (int)GetLastError());
Huffman_Uncompress((unsigned char*)readBuf, (unsigned char*)unCompressed, headerBuffer->lDataLength, headerBuffer->lDataUncompressed);
// For testing purposes.
emit error (QString("We have a Huffman buffer."),0);
}else if (headerBuffer->bVersion == 0xF0)
{
if(!(unCompressed = (char *)calloc(headerBuffer->lDataUncompressed,sizeof(char))))
emit error(QString("Error malloccing unCompressed."), (int)GetLastError());
unCompressed = RunLengthDecode(readBuf, headerBuffer->lDataLength);
// For testing purposes.
emit error (QString("We have an RLE buffer."),0);
}else if (headerBuffer->bVersion == 0x0F)
{
if(!(unCompressed = (char *)calloc(headerBuffer->lDataUncompressed,sizeof(char))))
emit error(QString("Error malloccing unCompressed."), (int)GetLastError());
unCompressed = (char*)DifferentialExpand(readBuf, headerBuffer->lDataLength);
// For testing purposes.
emit error (QString("We have a Differential buffer."),0);
}
else
emit error (QString("We have an uncompressed buffer."),0);
receiveID = GetReceiverId(headerBuffer->lReceiverAddr);
if (headerBuffer->bDataType == 0)
{ // If the data is text.
// create a new message structure and put it on the queue
// not all the header options we need are available - ask Jack!
if(!(newMsg = (Msg *)malloc(sizeof(struct message))))
emit error(QString("Error malloccing newMsg."), (int)GetLastError());
strcpy(newMsg->txt, unCompressed);
newMsg->senderID = headerBuffer->bSenderAddr;
newMsg->receiverID = (short)receiveID;
newMsg->msgNum = rand() % 100;
newMsg->priority = headerBuffer->bPriority;
AddToQueue(newMsg);
// Check if the senderID has already been created. If not, create one.
if ((sender = BSTSearch(root, headerBuffer->bSenderAddr))== NULL)
{
Item * newItem = (Item*)(malloc (sizeof(Item)));
int * count = (int*)(malloc (sizeof(int)));
*count = 1;
newItem->key = headerBuffer->bSenderAddr;
newItem->data = count;
root = BSTInsert(root,newItem);
}
else // If it has been created, increment
*((int*)sender->data) = *((int*)sender->data) + 1;
emit labelEdit(QString("Number of Messages: %1").arg(numberOfMessages));
}
else
{
// we have audio, emit the data, the length of the data, and the sample rate
emit audioReceived(headerBuffer->lDataUncompressed,
unCompressed,
headerBuffer->sSamplesPerSec);
}
}
}
else
{
// in raw mode, just grab chunks of bytes as they come
do
{
if(!(rawByte = (char *)calloc(1, sizeof(char))))
emit error(QString("Error malloccing rawByte."), (int)GetLastError());
if(!ReadFile(hComm, rawByte, 1, &dwBytesTransferred, 0))
emit error(QString("Error getting the raw data."), (int)GetLastError());
if(dwBytesTransferred != 0)
emit messageEdit(*rawByte);
}while(dwBytesTransferred != 0);
}
}
}
emit finished();
}
void BucketRanges::ResetChecksum() {
checksum_ = CalculateChecksum();
}
void GetGPSData(void)
{
union longbbbb Temp4;
union intbb Temp2;
float phi, theta, psi;
phi = (float)((XPLMGetDataf(drPhi) / 180) * PI);
theta = (float)((XPLMGetDataf(drTheta) / 180) * PI);
psi = (float)((XPLMGetDataf(drPsi) / 180) * PI);
int LocalDays = XPLMGetDatai(drLocalDays);
float LocalSecsFloat = XPLMGetDataf(drLocalSecs) * 1000;
LocalDays += 5;
int Week = (int)(LocalDays / 7) + 1564;
LocalDays = (LocalDays % 7);
Week = (Week * 10) + LocalDays;
int LocalSecsInt = (int)LocalSecsFloat + (LocalDays * 86400000);
LocalSecsFloat = (LocalSecsFloat - (int)LocalSecsFloat) * 1000000;
Temp2.BB = Week;
Store2LE(&NAV_SOL[14], Temp2);
Temp4.WW = LocalSecsInt;
Store4LE(&NAV_SOL[6], Temp4);
Store4LE(&NAV_DOP[6], Temp4);
Store4LE(&NAV_POSLLH[6], Temp4);
Store4LE(&NAV_VELNED[6], Temp4);
Temp4.WW = (int)LocalSecsFloat;
Store4LE(&NAV_SOL[10], Temp4);
double latitude = XPLMGetDataf(drLat);
double longitude = XPLMGetDataf(drLon);
double elevation = XPLMGetDataf(drElev);
double local_x = XPLMGetDataf(drLocal_x);
double local_y = XPLMGetDataf(drLocal_y);
double local_z = XPLMGetDataf(drLocal_z);
double local_vx = XPLMGetDataf(drLocal_vx);
double local_vy = XPLMGetDataf(drLocal_vy);
double local_vz = XPLMGetDataf(drLocal_vz);
Temp4.WW = (int)(local_vx * 100);
Store4LE(&NAV_VELNED[14], Temp4);
Temp4.WW = (int)(local_vy * -100);
Store4LE(&NAV_VELNED[18], Temp4);
Temp4.WW = (int)(local_vz * -100);
Store4LE(&NAV_VELNED[10], Temp4);
Temp4.WW = (int)(XPLMGetDataf(drAirSpeedTrue) * 100);
Store4LE(&NAV_VELNED[22], Temp4);
// note: xplane ground speed is not GPS speed over ground,
// it is 3D ground speed. we need horizontal ground speed for GPS,
// which is computed from the horizontal local velocity components:
double speed_over_ground = 100 * sqrt(local_vx*local_vx + local_vz*local_vz);
Temp4.WW = (int)speed_over_ground;
Store4LE(&NAV_VELNED[26], Temp4);
// Compute course over ground, in degrees,
// from horizontal earth frame velocities,
// which are in OGL frame of reference.
// local_vx is east, local_vz is south.
double course_over_ground = (atan2(local_vx, -local_vz) / PI * 180.0);
// MatrixPilot is expecting an angle between 0 and 360 degrees.
if (course_over_ground < 0.0) course_over_ground += 360.0;
Temp4.WW = (int)(100000.0 * course_over_ground);
Store4LE(&NAV_VELNED[30], Temp4);
Temp4.WW = (int)(latitude * 10000000);
Store4LE(&NAV_POSLLH[14], Temp4);
Temp4.WW = (int)(longitude * 10000000);
Store4LE(&NAV_POSLLH[10], Temp4);
Temp4.WW = (int)(elevation * 1000);
Store4LE(&NAV_POSLLH[18], Temp4);
Store4LE(&NAV_POSLLH[22], Temp4);
double ac_pos_lat, ac_pos_lon, ac_pos_elev;
double ac_vel_lat, ac_vel_lon, ac_vel_elev;
// Get AC pos in LLA
XPLMLocalToWorld(local_x,
local_y,
local_z,
&ac_pos_lat,
&ac_pos_lon,
&ac_pos_elev);
// Get AC pos + velocity vector in LLA
XPLMLocalToWorld(local_x + local_vx,
local_y + local_vy,
local_z + local_vz,
&ac_vel_lat,
&ac_vel_lon,
&ac_vel_elev);
// convert to ECEF
LLAtoECEF(ac_pos_lat, ac_pos_lon, ac_pos_elev, local_x, local_y, local_z);
LLAtoECEF(ac_vel_lat, ac_vel_lon, ac_vel_elev, local_vx, local_vy, local_vz);
// AC pos stays as is
// subtract to get velocity vector in ECEF
local_vy -= local_y;
local_vx -= local_x;
local_vz -= local_z;
Temp4.WW = (int)(local_x * 100);
Store4LE(&NAV_SOL[18], Temp4);
Temp4.WW = (int)(local_y * 100);
Store4LE(&NAV_SOL[22], Temp4);
Temp4.WW = (int)(local_z * 100);
Store4LE(&NAV_SOL[26], Temp4);
Temp4.WW = (int)(local_vx * 100);
Store4LE(&NAV_SOL[34], Temp4);
Temp4.WW = (int)(local_vy * 100);
Store4LE(&NAV_SOL[38], Temp4);
Temp4.WW = (int)(local_vz * 100);
Store4LE(&NAV_SOL[42], Temp4);
// simulate the magnetometer, and place in slots 30,32 and 46 of NAV_SOL
// these slots are used by Ublox, but not by HILSIM
// computation is based on zero declination, and zero inclination
float mag_field_x = 0.0; // earth OGL x mag field (east)
float mag_field_y = 0.0; // earth OGL y mag field (up)
float mag_field_z = -MAG_FIELD; // earth OGL z mag field (south)
// note, the "north pole" of the earth is really a south magnetic pole
// convert to NED body frame
OGLtoBCBF(mag_field_x, mag_field_y, mag_field_z, phi, theta, psi);
// convert from NED body to UDB body frame
double mag_field_body_udb_x = -mag_field_y;
double mag_field_body_udb_y = mag_field_x;
double mag_field_body_udb_z = mag_field_z;
// store in unused slots in NAV_SOL message
Temp2.BB = (int)mag_field_body_udb_x;
Store2LE(&NAV_SOL[30], Temp2);
Temp2.BB = (int)mag_field_body_udb_y;
Store2LE(&NAV_SOL[32], Temp2);
Temp2.BB = (int)mag_field_body_udb_z;
Store2LE(&NAV_SOL[46], Temp2);
CalculateChecksum(NAV_SOL);
SendToComPort(sizeof(NAV_SOL), NAV_SOL);
CalculateChecksum(NAV_DOP);
SendToComPort(sizeof(NAV_DOP), NAV_DOP);
CalculateChecksum(NAV_POSLLH);
SendToComPort(sizeof(NAV_POSLLH), NAV_POSLLH);
CalculateChecksum(NAV_VELNED);
SendToComPort(sizeof(NAV_VELNED), NAV_VELNED);
}
static bool
CheckChecksum(const void *data, size_t length)
{
return CalculateChecksum(data, length) == 0;
}
//-------------------------------------------------------------------
//----------------------------Code-----------------------------------
//-------------------------------------------------------------------
void main(void)
{
// should only execute main loop once; after this just respond to interrupts
InitTimers();
InitPorts();
InitComm();
InitInterrupts();
SSPBUF = BytesOut[i];
while(1)
{
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//-----EUART STUFF-------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
//-----------------------------------------------------------------------------------------
if(CommActive)
{
if( X_Transition!=0 )
{
X_Transition = 0;
switch( CurrentX_State )
{
case X_Idle_State :
{
SEND = 1;
NextX_State = X_StartDelim_State;
NextByteOut = 0x7E;
// Starting Edit for Variable Message Size
MessageCounter = Encode_Message(CurrentType);
X_LengthLSB = MessageCounter + 5; //For framing bytes
// Populating the Message Frame
X_CMD = TXData[0];
X_RF1 = TXData[1];
X_RF2 = TXData[2];
X_RF3 = TXData[3];
X_RF4 = TXData[4];
X_RF5 = TXData[5];
}
break;
case X_StartDelim_State :
{
NextX_State = X_LengthMSB_State;
NextByteOut = X_LengthMSB ;
}
break;
case X_LengthMSB_State :
{
NextX_State = X_LengthLSB_State;
NextByteOut = X_LengthLSB;
}
break;
case X_LengthLSB_State :
{
NextX_State = X_API_State;
NextByteOut = X_API;
}
break;
case X_API_State :
{
NextX_State = X_FID_State;
NextByteOut = X_FID;
}
break;
case X_FID_State :
{
NextX_State = X_AddrMSB_State;
NextByteOut = X_AddrMSB;
}
break;
case X_AddrMSB_State :
{
NextX_State = X_AddrLSB_State;
NextByteOut = X_AddrLSB;
}
break;
case X_AddrLSB_State :
{
NextX_State = X_Options_State;
NextByteOut = X_Options;
}
break;
case X_Options_State :
{
NextX_State = X_CMD_State; // will later need logic here for what transition to do based on length LSB
NextByteOut = X_CMD;
}
break;
case X_CMD_State :
{
MessageCounter--;
if(MessageCounter)
{
NextX_State = X_RF1_State;
NextByteOut = X_RF1;
}
else
{
NextX_State = X_Checksum_State;
NextByteOut = CalculateChecksum();
}
}
break;
case X_RF1_State :
{
MessageCounter--;
if(MessageCounter)
{
NextX_State = X_RF2_State;
NextByteOut = X_RF2;
}
else
{
NextX_State = X_Checksum_State;
NextByteOut = CalculateChecksum();
}
}
break;
case X_RF2_State :
{
MessageCounter--;
if(MessageCounter)
{
NextX_State = X_RF3_State;
NextByteOut = X_RF3;
}
else
{
NextX_State = X_Checksum_State;
NextByteOut = CalculateChecksum();
}
}
break;
case X_RF3_State :
{
MessageCounter--;
if(MessageCounter)
{
NextX_State = X_RF4_State;
NextByteOut = X_RF4;
}
else
{
NextX_State = X_Checksum_State;
NextByteOut = CalculateChecksum();
}
}
break;
case X_RF4_State :
{
MessageCounter--;
if(MessageCounter)
{
NextX_State = X_RF5_State;
NextByteOut = X_RF5;
}
else
{
NextX_State = X_Checksum_State;
NextByteOut = CalculateChecksum();
}
}
break;
case X_RF5_State :
{
NextX_State = X_Checksum_State;
NextByteOut = CalculateChecksum();
}
break;
case X_Checksum_State :
{
SEND = 0;
NextX_State = X_Idle_State;
NextByteOut = 0x7E;
}
break;
} // end switch case
}// end if X_Transition
if( R_Transition!=0 )
{
R_Transition = 0;
switch( CurrentR_State )
{
case R_Idle_State :
{
if (ByteIn == 0x7E) // should always be this!
{ NextR_State = R_LengthMSB_State; }
}
break;
case R_LengthMSB_State :
{
if ( ByteIn == 0x00 ) // should always be this!
{ NextR_State = R_LengthLSB_State; }
}
break;
case R_LengthLSB_State :
{
NextR_State = R_API_State;
R_LengthLSB = ByteIn;
}
break;
case R_API_State :
{
///Adding Code to Handle different types of received messages
R_API = ByteIn;
if (R_API == API_TXSTAT)
NextR_State = R_FID_State;
else if (R_API == API_RXPACK)
{
NextR_State = R_AddrMSB_State;
LostCommCounter = 0;
}
else
NextR_State = R_Idle_State;
}
break;
case R_AddrMSB_State :
{
NextR_State = R_AddrLSB_State;
R_AddrMSB = ByteIn;
}
break;
case R_AddrLSB_State :
{
NextR_State =R_SigStr_State;
R_AddrLSB = ByteIn;
}
break;
case R_SigStr_State :
{
NextR_State = R_Options_State;
R_SigStr = ByteIn;
}
break;
case R_Options_State :
{
NextR_State = R_CMD_State; // will later need logic here for what transition to do based on length LSB
R_Options = ByteIn;
}
break;
case R_CMD_State:
{
NextR_State = R_RF1_State;
R_CMD = ByteIn;
}
break;
case R_RF1_State :
{
NextR_State = R_Idle_State;
R_RF1 = ByteIn;
if (R_CMD == 0x05)
{ //PAIR_RESP Received
if(R_RF1&BIT0HI)
{
Paired = 1;
//Stop Broadcasting and send Direct Messages
X_AddrMSB = R_AddrMSB;
X_AddrLSB = R_AddrLSB;
CurrentType = CTRL;
Set_PAIRED;
PORTC = PORTC_Copy;
LostCommCounter = 0;
}
else
{
XBee_State(0);
}
}
if (R_CMD == 0x06)
{ //STATUS Received
Process_Status(R_RF1);
}
}
break;
case R_FID_State :
{
NextR_State = R_STATUS_State;
R_FID = ByteIn;
}
break;
case R_STATUS_State :
{
NextR_State = R_Idle_State;
R_STATUS = ByteIn;
//Code to Resend Message if no ACK
if (R_STATUS)
{
X_Transition = 1;
CurrentX_State = X_Idle_State;
NextX_State = CurrentX_State;
}
}
break;
} // end switch case
}// end if R_Transition
if (SEND && TXIF) // for some reason, with TXIE I get 0x7E transmitted twice.
{
X_Transition = 1;
TXREG = NextByteOut;
}
CurrentX_State = NextX_State;
CurrentR_State = NextR_State;
}
}
}; // end main
void appft(int niter, double *total_time, logical *verified)
{
int i, j, k, kt, n12, n22, n32, ii, jj, kk, ii2, ik2;
double ap;
dcomplex exp1[NX], exp2[NY], exp3[NZ];
for (i = 1; i <= 15; i++) {
timer_clear(i);
}
timer_start(2);
compute_initial_conditions(NX, NY, NZ, xnt);
CompExp(NX, exp1);
CompExp(NY, exp2);
CompExp(NZ, exp3);
fftXYZ(1, NX, NY, NZ, xnt, (dcomplex *)y, exp1, exp2, exp3);
timer_stop(2);
timer_start(1);
if (timers_enabled) timer_start(13);
n12 = NX / 2;
n22 = NY / 2;
n32 = NZ / 2;
ap = -4.0 * ALPHA * (PI * PI);
for (i = 0; i < NZ; i++) {
ii = i - (i / n32) * NZ;
ii2 = ii * ii;
for (k = 0; k < NY; k++) {
kk = k - (k / n22) * NY;
ik2 = ii2 + kk*kk;
for (j = 0; j < NX; j++) {
jj = j - (j / n12) * NX;
twiddle[i][k][j] = exp(ap*(double)(jj*jj + ik2));
}
}
}
if (timers_enabled) timer_stop(13);
if (timers_enabled) timer_start(12);
compute_initial_conditions(NX, NY, NZ, xnt);
if (timers_enabled) timer_stop(12);
if (timers_enabled) timer_start(15);
fftXYZ(1, NX, NY, NZ, xnt, (dcomplex *)y, exp1, exp2, exp3);
if (timers_enabled) timer_stop(15);
for (kt = 1; kt <= niter; kt++) {
if (timers_enabled) timer_start(11);
evolve(NX, NY, NZ, xnt, y, twiddle);
if (timers_enabled) timer_stop(11);
if (timers_enabled) timer_start(15);
fftXYZ(-1, NX, NY, NZ, xnt, (dcomplex *)xnt, exp1, exp2, exp3);
if (timers_enabled) timer_stop(15);
if (timers_enabled) timer_start(10);
CalculateChecksum(&sums[kt], kt, NX, NY, NZ, xnt);
if (timers_enabled) timer_stop(10);
}
// Verification test.
if (timers_enabled) timer_start(14);
verify(NX, NY, NZ, niter, sums, verified);
if (timers_enabled) timer_stop(14);
timer_stop(1);
*total_time = timer_read(1);
if (!timers_enabled) return;
printf(" FT subroutine timers \n");
printf(" %26s =%9.4f\n", "FT total ", timer_read(1));
printf(" %26s =%9.4f\n", "WarmUp time ", timer_read(2));
printf(" %26s =%9.4f\n", "fftXYZ body ", timer_read(3));
printf(" %26s =%9.4f\n", "Swarztrauber ", timer_read(4));
printf(" %26s =%9.4f\n", "X time ", timer_read(7));
printf(" %26s =%9.4f\n", "Y time ", timer_read(8));
printf(" %26s =%9.4f\n", "Z time ", timer_read(9));
printf(" %26s =%9.4f\n", "CalculateChecksum ", timer_read(10));
printf(" %26s =%9.4f\n", "evolve ", timer_read(11));
printf(" %26s =%9.4f\n", "compute_initial_conditions", timer_read(12));
printf(" %26s =%9.4f\n", "twiddle ", timer_read(13));
printf(" %26s =%9.4f\n", "verify ", timer_read(14));
printf(" %26s =%9.4f\n", "fftXYZ ", timer_read(15));
printf(" %26s =%9.4f\n", "Benchmark time ", *total_time);
}
/* ReadWrite - Takes care of all read and write commands.
* Return - For write, returns length of packet that was successfully sent out.
* - For read, returns length of packet that was succesfully read.
* - If unsuccessful, returns a bad number (-99999)
*/
int DarwinController::ReadWrite(unsigned char *txpacket, unsigned char *rxpacket){
int buf = 30;
int length = 0;
int count = 0;
unsigned char info[MAXNUM_RXPARAM] = {0, };
length = txpacket[LENGTH] + 4;
int return_length = -99999;
int to_length = 0;
int num = 0;
port.ClearPort();
if(port.WritePort(txpacket, length) == length){ // if successfully writes out the entire packet
//printf("in readwrite length: %d\n", length); // for debug purposes
if(txpacket[INSTRUCTION] == SYNC_WRITE){
return length;
}
if(txpacket[INSTRUCTION] == BULK_READ){ //set bulk read vars
num = (txpacket[LENGTH]-3) / 3;
for(int i = 0; i < num; i++){
int _id = txpacket[PARAMETER+(3*i)+2];
int _len = txpacket[PARAMETER+(3*i)+1];
int _addr = txpacket[PARAMETER+(3*i)+3];
to_length += _len + 6;
// uncomment if you want to use bulkreaddata structure
//BulkData[_id].length = _len;
//BulkData[_id].start_address = _addr;
}
} else if(txpacket[INSTRUCTION] == READ){
to_length = txpacket[PARAMETER+1] + 6;
} else {
to_length = 6;
}
int get_length = 0; // total received packet length
int fail_counter = 0; // fail counter for ping
int length = 0; //received current packet length
//printf("getlength: %d, tolength: %d\n", get_length, to_length); // for debug purposes
// set packet time out:
double packetStartTime = Time.getCurrentTime();
double packetWaitTime = 0.012 * (double)length + 5.0;
while(1){ // loop for receiving packet
if(fail_counter >= 5){ //failed 5 times. return.
if(txpacket[INSTRUCTION] == PING){
printf("failed ping\n");
}
return 0; // culprit. things go wrong if this returns -1.
}
length = port.ReadPort(&rxpacket[get_length], to_length - get_length);
get_length += length; // get_length is how long the received packet is
if(get_length == to_length){ //received a packet of correct length
if(txpacket[INSTRUCTION] == BULK_READ){
break;
} else if(rxpacket[0] == 0xFF && rxpacket[1] == 0xFF){
//check checksum of incoming packet
unsigned char checksum = CalculateChecksum(rxpacket);
if(rxpacket[get_length -1] == checksum){
if(txpacket[INSTRUCTION] == PING){
printf("Successful ping\n");
}
//successful read / ping
return length;
}
}
// didn't get packet, write out to port again
get_length = 0;
port.ClearPort();
port.WritePort(txpacket, length);
} else { //check time out status
if(Time.isTimeOut(packetStartTime, packetWaitTime)){
if(get_length == 0){
//printf("timed out\n");
} else {
//printf("rxpacket corrupt\n");
}
if(txpacket[INSTRUCTION] != BULK_READ){
fail_counter++;
get_length = 0;
packetStartTime = Time.getCurrentTime();
port.ClearPort();
port.WritePort(txpacket, length);
}
}
}
}
if(txpacket[INSTRUCTION] != BULK_READ){
// if it isn't bulkread, must return here
return length;
}
/*
for(int i = 0; i < num; i++){
int _id = txpacket[PARAMETER+(3*i)+2];
BulkData[_id].error = -1;
}
*/
return_length = get_length; // make a copy of the received length for return purposes
while(1){ // this loop is purely for bulkread
int i;
for(i = 0; i< get_length - 1; i++){ //find the header
if(rxpacket[i] == 0xFF && rxpacket[i+1] == 0xFF){
break;
} else if(i == (get_length - 2) && rxpacket[get_length - 1] == 0xFF){
break;
}
}
if(i == 0){ //header is at beginning of packet
// Check checksum
unsigned char checksum = CalculateChecksum(rxpacket);
//printf("rxpacket[ID]: %d\n", rxpacket[ID]); // for debugging
if(rxpacket[LENGTH + rxpacket[LENGTH]] == checksum){
for(int j = 0; j < (rxpacket[LENGTH]-2); j++){
//BulkData[rxpacket[ID]].table[BulkData[rxpacket[ID]].start_address + j] = rxpacket[PARAMETER + j];
// printf("j: %d, rxpacket: %d\n", j, rxpacket[PARAMETER + j]); // for debugging
info[count++] = rxpacket[PARAMETER + j];
}
//BulkData[rxpacket[ID]].error = (int)rxpacket[ERRBIT];
int cur_packet_length = LENGTH + 1 + rxpacket[LENGTH];
to_length = get_length - cur_packet_length;
// shift things after the first packet up to the beginning of the packet
for(int j = 0; j <= to_length; j++){
rxpacket[j] = rxpacket[j+cur_packet_length];
}
get_length = to_length;
num--;
} else {
//printf("rx corrupt\n");
for(int j = 0; j <= to_length - 2; j++){
rxpacket[j] = rxpacket[j+2];
}
to_length = get_length -= 2;
}
if(num == 0){ // got all of the packets
break;
} else if(get_length <= 6) {
if(num != 0){
// printf("rx corrupt\n");
}
break;
}
} else { // try to salvage packets
for(int j = 0; j < (get_length - i); j++){
rxpacket[j] = rxpacket[j+i];
}
get_length -= i;
}
}
if(BulkReadJointData){
// Sort into structs here
for(int i = 0; i<20; i++){
ReadData& rd = jointRead[i];
rd.d = info[buf];
rd.i = info[buf+1];
rd.p = info[buf+2];
rd.goal_pos = MakeWord(info[buf+4], info[buf+5]);
rd.max_speed = MakeWord(info[buf+6], info[buf+7]);
rd.torque_limit = MakeWord(info[buf+8], info[buf+9]);
rd.cur_pos = MakeWord(info[buf+10], info[buf+11]);
rd.cur_speed = MakeWord(info[buf+12], info[buf+13]);
rd.load = MakeWord(info[buf+14], info[buf+15]);
rd.registered = info[buf+18];
rd.moving = info[buf+20];
// 23 uchars per motor
buf = buf + 23;
}
}
return return_length;
}
return return_length;
}
uint32_t ExTCPHeader(IPSTACK * pIpStack, bool fStartsInMachineOrder)
{
uint16_t sum = ~(ippnTCP << 8);
uint16_t cbT = pIpStack->cbTranportHeader + pIpStack->cbPayload;
// checksum of the psuedo header
// remember dataOffset is a byte, so we don't care if in machine or network order
ExEndian(&cbT, sizeof(cbT));
sum = CalculateChecksum(sum, &cbT, sizeof(cbT));
if(ILIsIPv6(pIpStack->pLLAdp))
{
sum = CalculateChecksum(sum, &pIpStack->pIPv6Hdr->ipSrc, sizeof(IPv6));
sum = CalculateChecksum(sum, &pIpStack->pIPv6Hdr->ipDest, sizeof(IPv6));
}
else
{
sum = CalculateChecksum(sum, &pIpStack->pIPv4Hdr->ipSrc, sizeof(IPv4));
sum = CalculateChecksum(sum, &pIpStack->pIPv4Hdr->ipDest, sizeof(IPv4));
}
// must put in network order, and do checksum
if(fStartsInMachineOrder)
{
if(!ExTCPOptions(pIpStack->pTCPHdr))
{
return(ipsIpStackParsingError);
}
pIpStack->pTCPHdr->checksum = 0; // set this to zero for calcluation
ExEndian(&pIpStack->pTCPHdr->portSrc, sizeof(pIpStack->pTCPHdr->portSrc));
ExEndian(&pIpStack->pTCPHdr->portDest, sizeof(pIpStack->pTCPHdr->portDest));
ExEndian(&pIpStack->pTCPHdr->seqNbr, sizeof(pIpStack->pTCPHdr->seqNbr));
ExEndian(&pIpStack->pTCPHdr->ackNbr, sizeof(pIpStack->pTCPHdr->ackNbr));
ExEndian(&pIpStack->pTCPHdr->window, sizeof(pIpStack->pTCPHdr->window));
ExEndian(&pIpStack->pTCPHdr->urgentPtr, sizeof(pIpStack->pTCPHdr->urgentPtr));
sum = CalculateChecksum(sum, pIpStack->pTCPHdr, pIpStack->cbTranportHeader);
}
else
{
sum = CalculateChecksum(sum, pIpStack->pTCPHdr, pIpStack->cbTranportHeader);
// switch order
ExEndian(&pIpStack->pTCPHdr->portSrc, sizeof(pIpStack->pTCPHdr->portSrc));
ExEndian(&pIpStack->pTCPHdr->portDest, sizeof(pIpStack->pTCPHdr->portDest));
ExEndian(&pIpStack->pTCPHdr->seqNbr, sizeof(pIpStack->pTCPHdr->seqNbr));
ExEndian(&pIpStack->pTCPHdr->ackNbr, sizeof(pIpStack->pTCPHdr->ackNbr));
ExEndian(&pIpStack->pTCPHdr->window, sizeof(pIpStack->pTCPHdr->window));
ExEndian(&pIpStack->pTCPHdr->urgentPtr, sizeof(pIpStack->pTCPHdr->urgentPtr));
if(!ExTCPOptions(pIpStack->pTCPHdr))
{
return(ipsIpStackParsingError);
}
}
// add the data
pIpStack->pTCPHdr->checksum = CalculateChecksum(sum, pIpStack->pPayload, pIpStack->cbPayload);
// RFC 768, if zero and outgoing, make all FFs
if(fStartsInMachineOrder)
{
if(pIpStack->pTCPHdr->checksum == 0)
{
pIpStack->pTCPHdr->checksum = 0xFFFF;
}
}
else if(pIpStack->pTCPHdr->checksum != 0)
{
return(ipsIpStackChecksumError);
}
return(ipsSuccess);
}
bool BucketRanges::HasValidChecksum() const {
return CalculateChecksum() == checksum_;
}
status_t
MessageAdapter::_FlattenR5Message(uint32 format, const BMessage *from,
char *buffer, ssize_t *size)
{
BMessage::Private messagePrivate((BMessage *)from);
BMessage::message_header *header = messagePrivate.GetMessageHeader();
uint8 *data = messagePrivate.GetMessageData();
r5_message_header *r5header = (r5_message_header *)buffer;
uint8 *pointer = (uint8 *)buffer + sizeof(r5_message_header);
r5header->magic = MESSAGE_FORMAT_R5;
r5header->what = from->what;
r5header->checksum = 0;
uint8 flags = R5_MESSAGE_FLAG_VALID;
if (header->target != B_NULL_TOKEN) {
*(int32 *)pointer = header->target;
pointer += sizeof(int32);
flags |= R5_MESSAGE_FLAG_INCLUDE_TARGET;
}
if (header->reply_port >= 0 && header->reply_target != B_NULL_TOKEN
&& header->reply_team >= 0) {
// reply info
*(port_id *)pointer = header->reply_port;
pointer += sizeof(port_id);
*(int32 *)pointer = header->reply_target;
pointer += sizeof(int32);
*(team_id *)pointer = header->reply_team;
pointer += sizeof(team_id);
// big flags
*pointer = (header->reply_target == B_PREFERRED_TOKEN ? 1 : 0);
pointer++;
*pointer = (header->flags & MESSAGE_FLAG_REPLY_REQUIRED ? 1 : 0);
pointer++;
*pointer = (header->flags & MESSAGE_FLAG_REPLY_DONE ? 1 : 0);
pointer++;
*pointer = (header->flags & MESSAGE_FLAG_IS_REPLY ? 1 : 0);
pointer++;
flags |= R5_MESSAGE_FLAG_INCLUDE_REPLY;
}
if (header->flags & MESSAGE_FLAG_HAS_SPECIFIERS)
flags |= R5_MESSAGE_FLAG_SCRIPT_MESSAGE;
r5header->flags = flags;
// store the header size - used for the checksum later
ssize_t headerSize = (addr_t)pointer - (addr_t)buffer;
// collect and add the data
BMessage::field_header *field = messagePrivate.GetMessageFields();
for (uint32 i = 0; i < header->field_count; i++, field++) {
flags = R5_FIELD_FLAG_VALID;
if (field->count == 1)
flags |= R5_FIELD_FLAG_SINGLE_ITEM;
// ToDo: we don't really know the data size now (padding missing)
if (field->data_size <= 255 && field->count <= 255)
;//flags |= R5_FIELD_FLAG_MINI_DATA;
if (field->flags & FIELD_FLAG_FIXED_SIZE)
flags |= R5_FIELD_FLAG_FIXED_SIZE;
*pointer = flags;
pointer++;
*(type_code *)pointer = field->type;
pointer += sizeof(type_code);
if (!(flags & R5_FIELD_FLAG_SINGLE_ITEM)) {
if (flags & R5_FIELD_FLAG_MINI_DATA) {
*pointer = (uint8)field->count;
pointer++;
} else {
*(int32 *)pointer = field->count;
pointer += sizeof(int32);
}
}
// we may have to adjust this to account for padding later
uint8 *fieldSize = pointer;
if (flags & R5_FIELD_FLAG_MINI_DATA) {
*pointer = (uint8)field->data_size;
pointer++;
} else {
*(ssize_t *)pointer = field->data_size;
pointer += sizeof(ssize_t);
}
// name
int32 nameLength = min_c(field->name_length - 1, 255);
*pointer = (uint8)nameLength;
pointer++;
strncpy((char *)pointer, (char *)data + field->offset, nameLength);
pointer += nameLength;
// data
uint8 *source = data + field->offset + field->name_length;
if (flags & R5_FIELD_FLAG_FIXED_SIZE) {
memcpy(pointer, source, field->data_size);
pointer += field->data_size;
} else {
uint8 *previous = pointer;
for (uint32 i = 0; i < field->count; i++) {
ssize_t itemSize = *(ssize_t *)source + sizeof(ssize_t);
memcpy(pointer, source, itemSize);
ssize_t paddedSize = pad_to_8(itemSize);
memset(pointer + itemSize, 0, paddedSize - itemSize);
pointer += paddedSize;
source += itemSize;
}
// adjust the field size to the padded value
if (flags & R5_FIELD_FLAG_MINI_DATA)
*fieldSize = (uint8)(pointer - previous);
else
*(ssize_t *)fieldSize = (pointer - previous);
}
}
// terminate the fields with a pseudo field with flags 0 (not valid)
*pointer = 0;
pointer++;
// calculate the flattened size from the pointers
r5header->flattened_size = (addr_t)pointer - (addr_t)buffer;
r5header->checksum = CalculateChecksum((uint8 *)(buffer + 8),
headerSize - 8);
if (size)
*size = r5header->flattened_size;
return B_OK;
}
float GetBodyRates(float elapsedMe, float elapsedSim, int counter, void* refcon)
{
(void)elapsedSim;
(void)counter;
(void)refcon;
pendingElapsedTime += elapsedMe;
ReceiveFromComPort();
if (pendingElapsedTime < 0.025) // Don't run faster than 40Hz
{
return -1;
}
union intbb Temp2;
float phi, theta, psi;
float alpha, beta;
float P_flight, Q_flight, R_flight;
float ax, ay, az;
float gravity_acceleration_x, gravity_acceleration_y, gravity_acceleration_z;
// Angular rates in X-Plane are specified relative to the flight path, not to the aircraft,
// for reasons unknown. So that means we need to rotate by alpha and beta to get angular rates
// in the aircraft body frame, which is what the UDB measures.
// Retrieve rates and slip angles, and convert to radians
P_flight = XPLMGetDataf(drP) / 180 * PI;
Q_flight = XPLMGetDataf(drQ) / 180 * PI;
R_flight = XPLMGetDataf(drR) / 180 * PI;
alpha = XPLMGetDataf(drAlpha) / 180 * PI;
beta = XPLMGetDataf(drBeta) / 180 * PI;
// On 25th Jan 2015, Bill Premerlani confirmed with Austin Meyer, author of X-Plane
// that P, Q and R are rotations in the body frame. So they do not need to be rotated into
// any other frame of reference, other than a small sign correction for the UDB frame conventions.
// Austin Meyer said: "now, i CAN say that P is roll, Q is pitch, and R is yaw, all in degrees per second
//about the aircraft axis,..... (i just looked at the code to confirm this)"
P_plane = P_flight;
Q_plane = -Q_flight; // convert from NED to UDB
R_plane = R_flight;
// Angular rate
// multiply by 5632 (constant from UDB code)
// Divide by SCALEGYRO(3.0 for red board)
// 1 * 5632 / 3.0 = 1877.33
Temp2.BB = (int)(P_plane * 1877.33);
Store2LE(&NAV_BODYRATES[6], Temp2);
Temp2.BB = (int)(Q_plane * 1877.33);
Store2LE(&NAV_BODYRATES[8], Temp2);
Temp2.BB = (int)(R_plane * 1877.33);
Store2LE(&NAV_BODYRATES[10], Temp2);
// Our euler angles:
// X-Plane angles are in degrees.
// Phi is roll, roll to right is positive
// Theta is pitch, pitch up is positive
// Psi is yaw, relative to north. North is 0/360.
// Convert these angles to radians first.
phi = (float)((XPLMGetDataf(drPhi) / 180) * PI);
theta = (float)((XPLMGetDataf(drTheta) / 180) * PI);
psi = (float)((XPLMGetDataf(drPsi) / 180) * PI);
// set up a vertical reference for the plotting computations
// vertical in earth frame:
ax = 0;
ay = -(float)9.8;
az = 0;
// get the acceleration loading (gravity-acceleration) in the body frame in "g"s,
// and convert to meter/sec/sec
// x, y, and z are "UDB" coordinates, x is left wing, y is forward, and z is down.
gravity_acceleration_x = (float)((XPLMGetDataf(drg_side)) * 9.8);
gravity_acceleration_y = (float)((XPLMGetDataf(drg_axil)) * 9.8);
gravity_acceleration_z = (float)((XPLMGetDataf(drg_nrml)) * 9.8);
// Convert from OGL frame to Aircraft body fixed frame
// This produces a vertical reference in body frame
OGLtoBCBF(ax, ay, az, phi, theta, psi);
ax_plane = ax;
ay_plane = -ay; // convert from NED to UDB
az_plane = az;
// Lastly we need to convert from X-Plane units (m/s^2) to the arbitrary units used by the UDB
// Accelerations are in m/s^2
// Divide by 9.8 to get g's
// Multiply by 5280 (constant from UDB code)
// Divide by SCALEACCEL (2.64 for red board)
// 1 / 9.8 * 5280 / 2.64 = 204.8
Temp2.BB = (int)(gravity_acceleration_x * 204.8);
Store2LE(&NAV_BODYRATES[12], Temp2);
Temp2.BB = (int)(gravity_acceleration_y * 204.8);
Store2LE(&NAV_BODYRATES[14], Temp2);
Temp2.BB = (int)(gravity_acceleration_z * 204.8);
Store2LE(&NAV_BODYRATES[16], Temp2);
CalculateChecksum(NAV_BODYRATES);
SendToComPort(sizeof(NAV_BODYRATES), NAV_BODYRATES);
while (pendingElapsedTime >= 0.025) // Don't run slower than 40Hz
{
GPSCount++;
if (!IsConnected())
{
ConnectionCount++;
if (ConnectionCount % 160 == 0) // attempt reconnection every 4 seconds when disconnected
{
AttemptConnection();
ConnectionCount = 0;
}
}
if (GPSCount % 10 == 0)
{
GetGPSData();
GPSCount = 0;
}
pendingElapsedTime -= (float)0.025;
}
ServosToControls();
// float ThrottleSetting = 0; //SurfaceDeflections[CHANNEL_THROTTLE];
// float throttle[8] = {ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting,
// ThrottleSetting, ThrottleSetting, ThrottleSetting, ThrottleSetting};
XPLMSetDatavf(drThro, ThrottleSettings, 0, 8);
static float prevBrakeSetting = PARKBRAKE_ON;
if (BrakeSetting != prevBrakeSetting)
{
prevBrakeSetting = BrakeSetting;
XPLMSetDataf(drBrake, BrakeSetting);
// LoggingFile.mLogFile << "Set parkbrake to " << BrakeSetting << endl;
}
return -1; // get called back on every frame
}