/* 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 }