// 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]);
}
// 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);
}
}
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;
}
