/* 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;
}
Exemple #3
0
/*
================
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

}
Exemple #5
0
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);
}
Exemple #6
0
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;
}
Exemple #7
0
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();
}
Exemple #8
0
	void BucketRanges::ResetChecksum() {
		checksum_ = CalculateChecksum();
	}
Exemple #9
0
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
Exemple #12
0
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;
}
Exemple #14
0
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);
}
Exemple #15
0
	bool BucketRanges::HasValidChecksum() const {
		return CalculateChecksum() == checksum_;
	}
Exemple #16
0
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;
}
Exemple #17
0
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
}