예제 #1
0
파일: Accel.cpp 프로젝트: ns/quadcopter
// Calculate zero for all 3 axis, storing it for later measurements
// This assumes your platform is truly level!
// See also: http://www.freescale.com/files/sensors/doc/app_note/AN3447.pdf
void Accel::autoZero(){
  // Take 50 measurements of all 3 axis, find the median, that's our zero-point
  // Why 50? Because that's what the aeroquad project does
  byte loopCount = 50;
  
  //Serial.print("Starting accel autoZero with ");
  //Serial.print(loopCount, DEC);
  //Serial.println(" iterations.");
  int findZero[loopCount];
  for (byte axis = XAXIS; axis <= ZAXIS; axis++){
    for (byte i=0; i<loopCount; i++){
      sendReadRequest(0x32 + (axis * 2));
      findZero[i] = readWordFlip();
      delay(10);
    }
    
    zero[axis] = findMedian(findZero, loopCount);
    /*Serial.print("Zero of accel axis ");
    Serial.print(axis, DEC);
    Serial.print(" is: ");
    Serial.println(zero[axis]);*/
  }
  
  // Write to eeprom
//  eeprom_write(EEPROM_ADDR_ACCEL_PITCH, zero[XAXIS]);
//  eeprom_write(EEPROM_ADDR_ACCEL_ROLL, zero[YAXIS]);
//  eeprom_write(EEPROM_ADDR_ACCEL_YAW, zero[ZAXIS]);
}
예제 #2
0
파일: Accel.cpp 프로젝트: ns/quadcopter
// Updates all raw measurements from the accelerometer
void Accel::updateAll(){
  sendReadRequest(0x32);
  requestBytes(6);

  for (byte axis = XAXIS; axis <= ZAXIS; axis++) {
     dataRaw[axis] = zero[axis] - readNextWordFlip();
     dataSmoothed[axis] = filterSmooth(gConstant[axis] * dataRaw[axis] + gB[axis], dataSmoothed[axis], _smoothFactor);
  }
}
예제 #3
0
int main(int argc, char *argv[]) {
	int defaultParams = argc < 8;

	//start parameters
	if(defaultParams) {
		printf("1st parameter: number of nodes to read \n");
		printf("2nd parameter: number of read-tries \n");
		printf("3rd parameter: name of the file to save measurement data \n");
		printf("4th parameter: 1 = read same node, 0 = read different nodes \n");
		printf("5th parameter: ip adress \n");
		printf("6th parameter: port \n");
		printf("7th parameter: 0=stateful, 1=stateless\n");
		printf("8th parameter: 0=tcp, 1=udp (only with stateless calls)\n");
		printf("\nUsing default parameters. \n");
	}

	UA_UInt32 nodesToReadSize;
	UA_UInt32 tries;
	UA_Boolean alwaysSameNode;
	UA_ByteString reply;
	UA_ByteString_newMembers(&reply, 65536);
	UA_Boolean stateless;
	UA_Boolean udp;

	if(defaultParams)
		nodesToReadSize = 1;
	else
		nodesToReadSize = atoi(argv[1]);

	if(defaultParams)
		tries= 2;
	else
		tries = (UA_UInt32) atoi(argv[2]);

	if(defaultParams){
		alwaysSameNode = UA_TRUE;
	}else{
		if(atoi(argv[4]) != 0)
			alwaysSameNode = UA_TRUE;
		else
			alwaysSameNode = UA_FALSE;
	}

	if(defaultParams){
		stateless = UA_FALSE;
	}else{
		if(atoi(argv[7]) != 0)
			stateless = UA_TRUE;
		else
			stateless = UA_FALSE;
	}

	if(defaultParams){
		udp = UA_FALSE;
	}else{
		if(atoi(argv[8]) != 0)
			udp = UA_TRUE;
		else
			udp = UA_FALSE;
	}



    //Connect to remote server
	UA_String endpoint;
	UA_String_copycstring("none",&endpoint);
	ConnectionInfo connectionInfo;


/* REQUEST START*/
    UA_NodeId *nodesToRead;
    nodesToRead = UA_Array_new(&UA_TYPES[UA_TYPES_NODEID], 1);

	for(UA_UInt32 i = 0; i<1; i++) {
		if(alwaysSameNode)
			nodesToRead[i].identifier.numeric = 2253; //ask always the same node
		else
			nodesToRead[i].identifier.numeric = 19000 +i;
		nodesToRead[i].identifierType = UA_NODEIDTYPE_NUMERIC;
		nodesToRead[i].namespaceIndex = 0;
	}

	UA_DateTime tic, toc;
	UA_Double *timeDiffs;
	UA_Int32 received;
	timeDiffs = UA_Array_new(&UA_TYPES[UA_TYPES_DOUBLE], tries);
	UA_Double sum = 0;

	tic = UA_DateTime_now();

	/**
	UA_Double duration;

	UA_UInt32 count = 0;
	UA_Double start = 0, stop = 0;

	UA_UInt32 timeToRun = 30;
	UA_UInt32 timeToStart = 8;
	UA_UInt32 timeToStop = 22;

	do{
		toc = UA_DateTime_now();
		duration = ((UA_Double)toc-(UA_Double)tic)/(UA_Double)1e4;
		if(duration>=timeToStart*1000 && duration <= timeToStop*1000){
			if(start==0.0){
				start=UA_DateTime_now();
			}
		}
			//if(stateless || (!stateless && i==0)){
				if(defaultParams){
					if(ua_client_connectUA("127.0.0.1",atoi("16664"),&endpoint,&connectionInfo,stateless,udp) != 0){
						return 0;
					}
				}else{
					if(ua_client_connectUA(argv[5],atoi(argv[6]),&endpoint,&connectionInfo,stateless,udp) != 0){
						return 0;
					}
				}
			//}
			sendReadRequest(&connectionInfo,1,nodesToRead);
			received = recv(connectionInfo.socket, reply.data, 2000, 0);
			if(duration>=timeToStart*1000 && duration <= timeToStop*1000){
				count++;
			}

			if(!stateless){
			closeSession(&connectionInfo);
			recv(connectionInfo.socket, reply.data, 2000, 0);

			closeSecureChannel(&connectionInfo);
			}
			//if(stateless || (!stateless && i==tries-1)){
				close(connectionInfo.socket);
			//}
		if(duration >= timeToStop*1000 && stop==0){
			stop=UA_DateTime_now();
			printf("%i messages in %f secs, rate %f m/s\n", count, (stop-start)/(UA_Double)1e7, (UA_Double)count/((stop-start)/(UA_Double)1e7));
		}

	}while(duration<timeToRun*1000);

	exit(0);
	**/

	for(UA_UInt32 i = 0; i < tries; i++) {
		//if(stateless || (!stateless && i==0)){
		tic = UA_DateTime_now();
			if(defaultParams){
				if(ua_client_connectUA("127.0.0.1",atoi("16664"),&endpoint,&connectionInfo,stateless,udp) != 0){
					return 0;
				}
			}else{
				if(ua_client_connectUA(argv[5],atoi(argv[6]),&endpoint,&connectionInfo,stateless,udp) != 0){
					return 0;
				}
			}
		//}
		for(UA_UInt32 i = 0; i < nodesToReadSize; i++) {
		sendReadRequest(&connectionInfo,1,nodesToRead);
		received = recv(connectionInfo.socket, reply.data, 2000, 0);
		}
			if(!stateless){
		closeSession(&connectionInfo);
		recv(connectionInfo.socket, reply.data, 2000, 0);
			closeSecureChannel(&connectionInfo);
		}
		//if(stateless || (!stateless && i==tries-1)){
			close(connectionInfo.socket);
		//}
		toc = UA_DateTime_now() - tic;
		timeDiffs[i] = (UA_Double)toc/(UA_Double)1e4;
		sum = sum + timeDiffs[i];
	}

/* REQUEST END*/

	UA_Double mean = sum / tries;
	printf("mean time for handling request: %16.10f ms \n",mean);

	if(received>0)
		printf("received: %i\n",received); // dummy


	//save to file
	char data[100];
	const char flag = 'a';
	FILE* fHandle = UA_NULL;
	if (defaultParams) {
		fHandle =  fopen("client.log", &flag);
	}else{
		fHandle =  fopen(argv[3], &flag);
	}
	//header

	UA_Int32 bytesToWrite = sprintf(data, "measurement %s in ms, nodesToRead %d \n", argv[3], 1);
	fwrite(data,1,bytesToWrite,fHandle);
	for(UA_UInt32 i=0;i<tries;i++) {
		bytesToWrite = sprintf(data,"%16.10f \n",timeDiffs[i]);
		fwrite(data,1,bytesToWrite,fHandle);
	}
	fclose(fHandle);

	UA_String_deleteMembers(&reply);
	UA_Array_delete(nodesToRead,&UA_TYPES[UA_TYPES_NODEID], 1);
    UA_free(timeDiffs);

	return 0;
}