From 69133c1427be94303e8388b3e894ddaddd99b9b9 Mon Sep 17 00:00:00 2001 From: Laura Sebesta Date: Thu, 16 Aug 2012 00:13:19 +0200 Subject: [PATCH 01/12] Further optimized Matlab parsing. It now does most typecast as vectorized operations. The upshot is an approximately 50% performance boost. --- .../plugins/uavobjects/uavobjecttemplate.m | 125 ++++++++------ .../matlab/uavobjectgeneratormatlab.cpp | 156 +++++++++++++----- .../matlab/uavobjectgeneratormatlab.h | 3 +- 3 files changed, 189 insertions(+), 95 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m index 42d388a31..61b968bf1 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m @@ -59,45 +59,50 @@ $(ALLOCATIONCODE) fid = fopen(logfile); +buffer=fread(fid,Inf,'uchar=>uchar'); +fseek(fid, 0, 'bof'); + +bufferIdx=1; + correctMsgByte=hex2dec('20'); correctSyncByte=hex2dec('3C'); unknownObjIDList=zeros(1,2); % Parse log file, entry by entry -prebuf = fread(fid, 12, 'uint8'); -log_size = dir(logfile); -log_size = log_size.bytes; -last_print = 0; +% prebuf = buffer(1:12); + +last_print = -1e10; startTime=clock; while (1) - if (feof(fid)); break; end - %% Read message header % get sync field (0x3C, 1 byte) - sync = fread(fid, 1, 'uint8'); - if sync ~= correctSyncByte - prebuf = [prebuf(2:end); sync]; - wrongSyncByte = wrongSyncByte + 1; - continue - end - - %% Process header if we are aligned - timestamp = typecast(uint8(prebuf(1:4)), 'uint32'); - datasize = typecast(uint8(prebuf(5:12)), 'uint64'); + sync = buffer(bufferIdx+12); + + + if buffer(bufferIdx+12) ~= correctSyncByte + bufferIdx=bufferIdx+1; + wrongSyncByte = wrongSyncByte + 1; + continue + end + + %% Process header, if we are aligned +% timestamp = typecast(buffer(bufferIdx:bufferIdx + 4-1), 'uint32'); %We do this one later + datasizeBufferIdx = bufferIdx; %Just grab the index. We'll do a typecast later, if necessary +% sync = buffer(bufferIdx+12); %This one has already been done + msgType = buffer(bufferIdx+13); % get msg type (quint8 1 byte ) should be 0x20, ignore the rest? +% msgSize = typecast(buffer(bufferIdx+14:bufferIdx+ 14+2-1), 'uint16'); % NOT USED: get msg size (quint16 2 bytes) excludes crc, include msg header and data payload + objID = typecast(buffer(bufferIdx+16:bufferIdx+ 16+4-1), 'uint32'); % get obj id (quint32 4 bytes) - % get msg type (quint8 1 byte ) should be 0x20, ignore the rest? - msgType = fread(fid, 1, 'uint8'); + %Advance buffer past header + bufferIdx=bufferIdx+20; + + %Check that message type is correct if msgType ~= correctMsgByte wrongMessageByte = wrongMessageByte + 1; continue end - - % get msg size (quint16 2 bytes) excludes crc, include msg header and data payload - msgSize = fread(fid, 1, 'uint16'); - % get obj id (quint32 4 bytes) - objID = fread(fid, 1, 'uint32'); if (isempty(objID)) %End of file break; @@ -110,15 +115,18 @@ $(SWITCHCODE) otherwise unknownObjIDListIdx=find(unknownObjIDList(:,1)==objID, 1, 'first'); if isempty(unknownObjIDListIdx) - unknownObjIDList=[unknownObjIDList; objID 1]; + unknownObjIDList=[unknownObjIDList; uint32(objID) 1]; %#ok else - unknownObjIDList(unknownObjIDListIdx,2)=unknownObjIDList(unknownObjIDListIdx,2)+1; + unknownObjIDList(unknownObjIDListIdx,2)=unknownObjIDList(unknownObjIDListIdx,2)+1; end + + datasize = typecast(buffer(datasizeBufferIdx + 4:datasizeBufferIdx + 12-1), 'uint64'); + msgBytesLeft = datasize - 1 - 1 - 2 - 4; if msgBytesLeft > 255 msgBytesLeft = 0; end - fread(fid, msgBytesLeft, 'uint8'); + bufferIdx=bufferIdx+msgBytesLeft; end catch % One of the reads failed - indicates EOF @@ -126,42 +134,41 @@ $(SWITCHCODE) end if (wrongSyncByte ~= lastWrongSyncByte || wrongMessageByte~=lastWrongMessageByte ) ||... - (ftell(fid) / log_size - last_print) > 0.01 + bufferIdx - last_print > 5e4 %Every 50,000 bytes show the status update lastWrongSyncByte=wrongSyncByte; lastWrongMessageByte=wrongMessageByte; str1=[]; for i=1:length([str2 str3 str4 str5]); - str1=[str1 sprintf('\b')]; + str1=[str1 sprintf('\b')]; %#ok end str2=sprintf('wrongSyncByte instances: % 10d\n', wrongSyncByte ); str3=sprintf('wrongMessageByte instances: % 10d\n\n', wrongMessageByte ); - str4=sprintf('Completed bytes: % 9d of % 9d\n', ftell(fid), log_size); + str4=sprintf('Completed bytes: % 9d of % 9d\n', bufferIdx, length(buffer)); % Arbitrary times two so that it is at least as long - estTimeRemaining=(log_size-ftell(fid))/(ftell(fid)/etime(clock,startTime)) * 2; + estTimeRemaining=(length(buffer)-bufferIdx)/(bufferIdx/etime(clock,startTime)) * 2; h=floor(estTimeRemaining/3600); m=floor((estTimeRemaining-h*3600)/60); s=ceil(estTimeRemaining-h*3600-m*60); str5=sprintf('Est. time remaining, %02dh:%02dm:%02ds \n', h,m,s); - last_print = ftell(fid) / log_size; + last_print = bufferIdx; fprintf([str1 str2 str3 str4 str5]); end + %Check if at end of file. If not, load next prebuffer + if bufferIdx+12-1 > length(buffer) + break; + end +% bufferIdx=bufferIdx+12; - prebuf = fread(fid, 12, 'uint8'); end -fprintf('%d records in %0.2f seconds.\n', ftell(fid), etime(clock,startTime)); - -for i=2:size(unknownObjIDList,1) %Don't show the first one, as it was simply a dummy placeholder - disp(['Unknown object ID: 0x' dec2hex(unknownObjIDList(i,1),8) ' appeared ' int2str(unknownObjIDList(i,2)) ' times.']); -end for i=2:size(unknownObjIDList,1) %Don't show the first one, as it was simply a dummy placeholder disp(['Unknown object ID: 0x' dec2hex(unknownObjIDList(i,1),8) ' appeared ' int2str(unknownObjIDList(i,2)) ' times.']); @@ -170,28 +177,23 @@ end %% Clean Up and Save mat file fclose(fid); -% Trim output structs +%% Prune vectors $(CLEANUPCODE) + +%% Perform typecasting on vectors +$(FUNCTIONSCODE) + if strcmpi(outputType,'mat') matfile = strrep(logfile,'opl','mat'); save(matfile $(SAVEOBJECTSCODE)); else -$(EXPORTCSVCODE); +$(EXPORTCSVCODE) end +fprintf('%d records in %0.2f seconds.\n', length(buffer), etime(clock,startTime)); -%% Object reading functions -$(FUNCTIONSCODE) - -% This function prunes the excess pre-allocated space -function [structOut]=PruneStructOfArrays(structIn, lastIndex) - - fieldNames = fieldnames(structIn); - for i=1:length(fieldNames) - structOut.(fieldNames{i})=structIn.(fieldNames{i})(:,1:lastIndex); - end function OPLog2csv(structIn, structName, logfile) @@ -229,3 +231,28 @@ function crc = compute_crc(data) crc = crc_table(1+bitxor(data(i),crc)); end +function out=mcolon(inStart, inFinish) +%% This function was inspired by Bruno Luong's 'mcolon'. The name is kept the same as his 'mcolon' +% function, found on Matlab's file exchange. The two functions return identical +% results, although his is much faster. Unfortunately, C-compiled mex +% code would make this function non-cross-platform, so a Matlab scripted +% version is provided here. + if size(inStart,1) > 1 || size(inFinish,1) > 1 + if size(inStart,2) > 1 || size(inFinish,2) > 1 + error('Inputs must be vectors, i.e just one column wide.') + else + inStart=inStart'; + inFinish=inFinish'; + end + end + + diffIn=diff([inStart; inFinish]); + numElements=sum(diffIn)+length(inStart); + + out=zeros(1,numElements); + + idx=1; + for i=1:length(inStart) + out(idx:idx+diffIn(i))=inStart(i):inFinish(i); + idx=idx+diffIn(i)+1; + end diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp index 97e3e9e92..78fd102a1 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp @@ -31,7 +31,9 @@ using namespace std; bool UAVObjectGeneratorMatlab::generate(UAVObjectParser* parser,QString templatepath,QString outputpath) { fieldTypeStrMatlab << "int8" << "int16" << "int32" - << "uint8" << "uint16" << "uint32" << "float32" << "uint8"; + << "uint8" << "uint16" << "uint32" << "single" << "uint8"; + fieldSizeStrMatlab << "1" << "2" << "4" + << "1" << "2" << "4" << "4" << "1"; QDir matlabTemplatePath = QDir( templatepath + QString("ground/openpilotgcs/src/plugins/uavobjects")); QDir matlabOutputPath = QDir( outputpath + QString("matlab") ); @@ -46,7 +48,8 @@ bool UAVObjectGeneratorMatlab::generate(UAVObjectParser* parser,QString template for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { ObjectInfo* info=parser->getObjectByIndex(objidx); - process_object(info); + int numBytes=parser->getNumBytes(objidx); + process_object(info, numBytes); } matlabCodeTemplate.replace( QString("$(ALLOCATIONCODE)"), matlabAllocationCode); @@ -68,7 +71,7 @@ bool UAVObjectGeneratorMatlab::generate(UAVObjectParser* parser,QString template /** * Generate the matlab object files */ -bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info) +bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) { if (info == NULL) return false; @@ -82,12 +85,15 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info) QString functionCall(functionName + "(fid, timestamp, checkCRC, "); QString objectID(QString().setNum(info->id)); QString isSingleInst = boolTo01String( info->isSingleInst ); + QString numBytesString=QString("%1").arg(numBytes); //===================================================================// // Generate allocation code (will replace the $(ALLOCATIONCODE) tag) // //===================================================================// // matlabSwitchCode.append("\t\tcase " + objectID + "\n"); + + matlabAllocationCode.append("\n\t" + tableIdxName + " = 0;\n"); QString type; QString allocfields; @@ -121,6 +127,9 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info) } matlabAllocationCode.append(allocfields); matlabAllocationCode.append("\t" + objectTableName.toUpper() + "_OBJID=" + objectID + ";\n"); + matlabAllocationCode.append("\t" + objectTableName.toUpper() + "_NUMBYTES=" + numBytesString + ";\n"); + matlabAllocationCode.append("\t" + objectName + "FidIdx = [];\n"); + //==============================================================// @@ -130,8 +139,8 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info) matlabSwitchCode.append("\t\tcase " + objectTableName.toUpper() + "_OBJID\n"); -// matlabSwitchCode.append("\t\t\t" + objectTableName + "(" + tableIdxName +") = " + functionCall + ";\n"); - matlabSwitchCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName +" + 1;\n"); + matlabSwitchCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName +" + 1;\n"); + if(0){ matlabSwitchCode.append("\t\t\t" + objectTableName + "= " + functionCall + objectTableName + ", " + tableIdxName + ");\n"); matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName +".timestamp) %Check to see if pre-allocated memory is exhausted\n"); matlabSwitchCode.append("\t\t\t\tFieldNames= fieldnames(" + objectTableName +");\n"); @@ -139,12 +148,22 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info) matlabSwitchCode.append("\t\t\t\t\t" + objectTableName + ".(FieldNames{i})(:," + tableIdxName + "*2+1) = 0;\n"); matlabSwitchCode.append("\t\t\t\tend\n"); matlabSwitchCode.append("\t\t\tend\n"); +} else{ + matlabSwitchCode.append("\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + ") = bufferIdx; %#ok<*AGROW>\n"); + matlabSwitchCode.append("\t\t\tbufferIdx=bufferIdx + " + objectTableName.toUpper() + "_NUMBYTES+1; %+1 is for CRC\n"); + if(!info->isSingleInst){ + matlabSwitchCode.append("\t\t\tbufferIdx = bufferIdx + 2; %An extra two bytes for the instance ID\n"); + } + matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName +"FidIdx) %Check to see if pre-allocated memory is exhausted\n"); + matlabSwitchCode.append("\t\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + "*2) = 0;\n"); + matlabSwitchCode.append("\t\t\tend\n"); + } - //============================================================// - // Generate 'Cleanup:' code (will replace the $(CLEANUP) tag) // - //============================================================// - matlabCleanupCode.append(objectTableName + "=PruneStructOfArrays(" + objectTableName + "," + tableIdxName +");\n" ); +// //============================================================// +// // Generate 'Cleanup:' code (will replace the $(CLEANUP) tag) // +// //============================================================// + matlabCleanupCode.append(objectTableName + "FidIdx =" + objectTableName + "FidIdx(1:" + tableIdxName +");\n" ); //========================================================================// @@ -163,51 +182,98 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info) // Generate functions code (will replace the $(FUNCTIONSCODE) tag) // //=================================================================// //Generate function description comment - matlabFunctionsCode.append("%%\n% " + objectName + " read function\n"); - - matlabFunctionsCode.append("function [" + objectName + "] = " + functionCall + objectTableName + ", " + tableIdxName + ")" + "\n"); + if(0){ + matlabFunctionsCode.append("%%\n% " + objectName + " read function\n"); - - matlabFunctionsCode.append("\t" + objectName + ".timestamp(" + tableIdxName + ")= timestamp;\n"); - matlabFunctionsCode.append("\tif " + isSingleInst + "\n"); - matlabFunctionsCode.append("\t\theaderSize = 8;\n"); - matlabFunctionsCode.append("\telse\n"); - matlabFunctionsCode.append("\t\t" + objectName + ".instanceID(" + tableIdxName + ") = (fread(fid, 1, 'uint16'));\n"); - matlabFunctionsCode.append("\t\theaderSize = 10;\n"); - matlabFunctionsCode.append("\tend\n\n"); + matlabFunctionsCode.append("function [" + objectName + "] = " + functionCall + objectTableName + ", " + tableIdxName + ")" + "\n"); - // Generate functions code, actual fields of the object - QString funcfields; - matlabFunctionsCode.append("\tstartPos = ftell(fid) - headerSize;\n"); + matlabFunctionsCode.append("\t" + objectName + ".timestamp(" + tableIdxName + ")= timestamp;\n"); + matlabFunctionsCode.append("\tif " + isSingleInst + "\n"); + matlabFunctionsCode.append("\t\theaderSize = 8;\n"); + matlabFunctionsCode.append("\telse\n"); + matlabFunctionsCode.append("\t\t" + objectName + ".instanceID(" + tableIdxName + ") = (fread(fid, 1, 'uint16'));\n"); + matlabFunctionsCode.append("\t\theaderSize = 10;\n"); + matlabFunctionsCode.append("\tend\n\n"); - for (int n = 0; n < info->fields.length(); ++n) { - // Determine type - type = fieldTypeStrMatlab[info->fields[n]->type]; - // Append field - if ( info->fields[n]->numElements > 1 ) - funcfields.append("\t" + objectName + "." + info->fields[n]->name + "(:," + tableIdxName + ") = double(fread(fid, " + QString::number(info->fields[n]->numElements, 10) + ", '" + type + "'));\n"); - else - funcfields.append("\t" + objectName + "." + info->fields[n]->name + "(" + tableIdxName + ") = double(fread(fid, 1, '" + type + "'));\n"); + // Generate functions code, actual fields of the object + QString funcfields; + + matlabFunctionsCode.append("\tstartPos = ftell(fid) - headerSize;\n"); + + for (int n = 0; n < info->fields.length(); ++n) { + // Determine type + type = fieldTypeStrMatlab[info->fields[n]->type]; + // Append field + if ( info->fields[n]->numElements > 1 ) + funcfields.append("\t" + objectName + "." + info->fields[n]->name + "(:," + tableIdxName + ") = double(fread(fid, " + QString::number(info->fields[n]->numElements, 10) + ", '" + type + "'));\n"); + else + funcfields.append("\t" + objectName + "." + info->fields[n]->name + "(" + tableIdxName + ") = double(fread(fid, 1, '" + type + "'));\n"); + } + matlabFunctionsCode.append(funcfields); + + matlabFunctionsCode.append("\tobjLen = ftell(fid) - startPos;\n"); + // matlabFunctionsCode.append(QString("\tobjLen - %1 -headerSize\n").arg(numBytes)); + + matlabFunctionsCode.append("\t% read CRC\n"); + matlabFunctionsCode.append("\tcrc_read = fread(fid, 1, '*uint8');\n"); + + matlabFunctionsCode.append("\tif checkCRC\n"); + matlabFunctionsCode.append("\t\tfseek(fid, startPos, 'bof');\n"); + matlabFunctionsCode.append("\t\tcrc_calc = compute_crc(uint8(fread(fid,objLen,'uint8')));\n"); + matlabFunctionsCode.append("\t\tfread(fid,1,'uint8');\n"); + matlabFunctionsCode.append("\t\tif (crc_calc ~= crc_read)\n"); + matlabFunctionsCode.append("\t\t\tdisp('CRC Error')\n"); + matlabFunctionsCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName + " - 1;\n"); + matlabFunctionsCode.append("\t\tend\n"); + matlabFunctionsCode.append("\tend\n"); + + matlabFunctionsCode.append("\n\n"); } - matlabFunctionsCode.append(funcfields); + else{ + matlabFunctionsCode.append("% " + objectName + " typecasting\n"); + QString funcfields; - matlabFunctionsCode.append("\tobjLen = ftell(fid) - startPos;\n"); + //Add timestamp + funcfields.append("\t" + objectName + ".timestamp = " + + "double(typecast(buffer(mcolon(" + objectName + "FidIdx " + "- 20, " + objectName + "FidIdx + 4-1 -20)), 'uint32'))';\n"); - matlabFunctionsCode.append("\t% read CRC\n"); - matlabFunctionsCode.append("\tcrc_read = fread(fid, 1, '*uint8');\n"); + int currentIdx=0; - matlabFunctionsCode.append("\tif checkCRC\n"); - matlabFunctionsCode.append("\t\tfseek(fid, startPos, 'bof');\n"); - matlabFunctionsCode.append("\t\tcrc_calc = compute_crc(uint8(fread(fid,objLen,'uint8')));\n"); - matlabFunctionsCode.append("\t\tfread(fid,1,'uint8');\n"); - matlabFunctionsCode.append("\t\tif (crc_calc ~= crc_read)\n"); - matlabFunctionsCode.append("\t\t\tdisp('CRC Error')\n"); - matlabFunctionsCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName + " - 1;\n"); - matlabFunctionsCode.append("\t\tend\n"); - matlabFunctionsCode.append("\tend\n"); + //Add instance, if necessary + if(!info->isSingleInst){ + funcfields.append("\t" + objectName + ".instanceID = " + + "double(typecast(buffer(mcolon(" + objectName + "FidIdx " + ", " + objectName + "FidIdx + 2-1)), 'uint16'))';\n"); + currentIdx+=2; + } - matlabFunctionsCode.append("\n\n"); + for (int n = 0; n < info->fields.length(); ++n) { + // Determine type + type = fieldTypeStrMatlab[info->fields[n]->type]; + + //Determine variable length + QString size = fieldSizeStrMatlab[info->fields[n]->type]; + // Append field + if ( info->fields[n]->numElements > 1 ){ + funcfields.append("\t" + objectName + "." + info->fields[n]->name + " = " + + "reshape(double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + + ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt()*info->fields[n]->numElements - 1) + ")), '" + type + "')), "+ QString::number(info->fields[n]->numElements, 10) + ", [] );\n"); + } + else{ + funcfields.append("\t" + objectName + "." + info->fields[n]->name + " = " + + "double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + + ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() - 1) + ")), '" + type + "'))';\n"); + } + currentIdx+=size.toInt()*info->fields[n]->numElements; + } + matlabFunctionsCode.append(funcfields); + matlabFunctionsCode.append("\n"); + + } + +// ActuatorSettings.ChannelUpdateFreq(:,actuatorsettingsIdx) = double(fread(fid, 4, 'uint16')); return true; } diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h index c4025a4f4..afeaf7410 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h @@ -35,7 +35,7 @@ public: bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); private: - bool process_object(ObjectInfo* info); + bool process_object(ObjectInfo* info, int numBytes); QString matlabAllocationCode; QString matlabSwitchCode; QString matlabCleanupCode; @@ -43,6 +43,7 @@ private: QString matlabExportCsvCode; QString matlabFunctionsCode; QStringList fieldTypeStrMatlab; + QStringList fieldSizeStrMatlab; }; From 699b2f30305b11907a53bffec0804d3aad5174b7 Mon Sep 17 00:00:00 2001 From: Laura Sebesta Date: Thu, 16 Aug 2012 08:37:26 +0200 Subject: [PATCH 02/12] Cleaned up variable names, fixed bug in saving whereby the a log file could be saved over if the extension were not "opl". --- .../plugins/uavobjects/uavobjecttemplate.m | 8 +- .../matlab/uavobjectgeneratormatlab.cpp | 243 ++++++------------ .../matlab/uavobjectgeneratormatlab.h | 4 +- 3 files changed, 87 insertions(+), 168 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m index 61b968bf1..29ea1e084 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.m @@ -55,7 +55,7 @@ if ~strcmpi(outputType,'mat') && ~strcmpi(outputType,'csv') error('Incorrect file format specified. Second argument must be ''mat'' or ''csv''.'); end -$(ALLOCATIONCODE) +$(INSTANTIATIONCODE) fid = fopen(logfile); @@ -182,10 +182,12 @@ $(CLEANUPCODE) %% Perform typecasting on vectors -$(FUNCTIONSCODE) +$(ALLOCATIONCODE) +%% Save data to file if strcmpi(outputType,'mat') - matfile = strrep(logfile,'opl','mat'); + [path, name]=fileparts(logfile); + matfile = fullfile(path,[name '.mat']); save(matfile $(SAVEOBJECTSCODE)); else $(EXPORTCSVCODE) diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp index 78fd102a1..97b89ad80 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp @@ -52,11 +52,11 @@ bool UAVObjectGeneratorMatlab::generate(UAVObjectParser* parser,QString template process_object(info, numBytes); } - matlabCodeTemplate.replace( QString("$(ALLOCATIONCODE)"), matlabAllocationCode); + matlabCodeTemplate.replace( QString("$(INSTANTIATIONCODE)"), matlabInstantiationCode); matlabCodeTemplate.replace( QString("$(SWITCHCODE)"), matlabSwitchCode); matlabCodeTemplate.replace( QString("$(CLEANUPCODE)"), matlabCleanupCode); matlabCodeTemplate.replace( QString("$(SAVEOBJECTSCODE)"), matlabSaveObjectsCode); - matlabCodeTemplate.replace( QString("$(FUNCTIONSCODE)"), matlabFunctionsCode); + matlabCodeTemplate.replace( QString("$(ALLOCATIONCODE)"), matlabAllocationCode); matlabCodeTemplate.replace( QString("$(EXPORTCSVCODE)"), matlabExportCsvCode); bool res = writeFile( matlabOutputPath.absolutePath() + "/OPLogConvert.m", matlabCodeTemplate ); @@ -81,91 +81,103 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) // QString objectTableName(objectName + "Objects"); QString objectTableName(objectName); QString tableIdxName(objectName.toLower() + "Idx"); - QString functionName("Read" + info->name + "Object"); - QString functionCall(functionName + "(fid, timestamp, checkCRC, "); QString objectID(QString().setNum(info->id)); - QString isSingleInst = boolTo01String( info->isSingleInst ); QString numBytesString=QString("%1").arg(numBytes); - + //===================================================================// // Generate allocation code (will replace the $(ALLOCATIONCODE) tag) // //===================================================================// - // matlabSwitchCode.append("\t\tcase " + objectID + "\n"); + QString type; + QString allocfields; - - matlabAllocationCode.append("\n\t" + tableIdxName + " = 0;\n"); - QString type; - QString allocfields; - if (0){ - matlabAllocationCode.append("\t" + objectTableName + ".timestamp = 0;\n"); - for (int n = 0; n < info->fields.length(); ++n) { - // Determine type - type = fieldTypeStrMatlab[info->fields[n]->type]; - // Append field - if ( info->fields[n]->numElements > 1 ) - allocfields.append("\t" + objectTableName + "(1)." + info->fields[n]->name + " = zeros(" + QString::number(info->fields[n]->numElements, 10) + ",1);\n"); - else - allocfields.append("\t" + objectTableName + "(1)." + info->fields[n]->name + " = 0;\n"); - } - } - else{ - matlabAllocationCode.append("\t" + objectTableName + "=struct('timestamp', 0"); - if (!info->isSingleInst) { - allocfields.append(",...\n\t\t 'instanceID', 0"); - } - for (int n = 0; n < info->fields.length(); ++n) { - // Determine type - type = fieldTypeStrMatlab[info->fields[n]->type]; - // Append field - if ( info->fields[n]->numElements > 1 ) - allocfields.append(",...\n\t\t '" + info->fields[n]->name + "', zeros(" + QString::number(info->fields[n]->numElements, 10) + ",1)"); - else - allocfields.append(",...\n\t\t '" + info->fields[n]->name + "', 0"); - } - allocfields.append(");\n"); + matlabInstantiationCode.append("\n\t" + tableIdxName + " = 0;\n"); + matlabInstantiationCode.append("\t" + objectTableName + "=struct('timestamp', 0"); + if (!info->isSingleInst) { + allocfields.append(",...\n\t\t 'instanceID', 0"); } - matlabAllocationCode.append(allocfields); - matlabAllocationCode.append("\t" + objectTableName.toUpper() + "_OBJID=" + objectID + ";\n"); - matlabAllocationCode.append("\t" + objectTableName.toUpper() + "_NUMBYTES=" + numBytesString + ";\n"); - matlabAllocationCode.append("\t" + objectName + "FidIdx = [];\n"); + for (int n = 0; n < info->fields.length(); ++n) { + // Determine type + type = fieldTypeStrMatlab[info->fields[n]->type]; + // Append field + if ( info->fields[n]->numElements > 1 ) + allocfields.append(",...\n\t\t '" + info->fields[n]->name + "', zeros(" + QString::number(info->fields[n]->numElements, 10) + ",1)"); + else + allocfields.append(",...\n\t\t '" + info->fields[n]->name + "', 0"); + } + allocfields.append(");\n"); + matlabInstantiationCode.append(allocfields); + matlabInstantiationCode.append("\t" + objectTableName.toUpper() + "_OBJID=" + objectID + ";\n"); + matlabInstantiationCode.append("\t" + objectTableName.toUpper() + "_NUMBYTES=" + numBytesString + ";\n"); + matlabInstantiationCode.append("\t" + objectName + "FidIdx = [];\n"); //==============================================================// // Generate 'Switch:' code (will replace the $(SWITCHCODE) tag) // //==============================================================// - - - matlabSwitchCode.append("\t\tcase " + objectTableName.toUpper() + "_OBJID\n"); matlabSwitchCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName +" + 1;\n"); - if(0){ - matlabSwitchCode.append("\t\t\t" + objectTableName + "= " + functionCall + objectTableName + ", " + tableIdxName + ");\n"); - matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName +".timestamp) %Check to see if pre-allocated memory is exhausted\n"); - matlabSwitchCode.append("\t\t\t\tFieldNames= fieldnames(" + objectTableName +");\n"); - matlabSwitchCode.append("\t\t\t\tfor i=1:length(FieldNames) %Grow structure\n"); - matlabSwitchCode.append("\t\t\t\t\t" + objectTableName + ".(FieldNames{i})(:," + tableIdxName + "*2+1) = 0;\n"); - matlabSwitchCode.append("\t\t\t\tend\n"); - matlabSwitchCode.append("\t\t\tend\n"); -} else{ - matlabSwitchCode.append("\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + ") = bufferIdx; %#ok<*AGROW>\n"); - matlabSwitchCode.append("\t\t\tbufferIdx=bufferIdx + " + objectTableName.toUpper() + "_NUMBYTES+1; %+1 is for CRC\n"); - if(!info->isSingleInst){ - matlabSwitchCode.append("\t\t\tbufferIdx = bufferIdx + 2; %An extra two bytes for the instance ID\n"); - } - matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName +"FidIdx) %Check to see if pre-allocated memory is exhausted\n"); - matlabSwitchCode.append("\t\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + "*2) = 0;\n"); - matlabSwitchCode.append("\t\t\tend\n"); + matlabSwitchCode.append("\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + ") = bufferIdx; %#ok<*AGROW>\n"); + matlabSwitchCode.append("\t\t\tbufferIdx=bufferIdx + " + objectTableName.toUpper() + "_NUMBYTES+1; %+1 is for CRC\n"); + if(!info->isSingleInst){ + matlabSwitchCode.append("\t\t\tbufferIdx = bufferIdx + 2; %An extra two bytes for the instance ID\n"); } - - -// //============================================================// -// // Generate 'Cleanup:' code (will replace the $(CLEANUP) tag) // -// //============================================================// - matlabCleanupCode.append(objectTableName + "FidIdx =" + objectTableName + "FidIdx(1:" + tableIdxName +");\n" ); + matlabSwitchCode.append("\t\t\tif " + tableIdxName + " >= length(" + objectTableName +"FidIdx) %Check to see if pre-allocated memory is exhausted\n"); + matlabSwitchCode.append("\t\t\t\t" + objectTableName + "FidIdx(" + tableIdxName + "*2) = 0;\n"); + matlabSwitchCode.append("\t\t\tend\n"); + //============================================================// + // Generate 'Cleanup:' code (will replace the $(CLEANUP) tag) // + //============================================================// + matlabCleanupCode.append(objectTableName + "FidIdx =" + objectTableName + "FidIdx(1:" + tableIdxName +");\n" ); + + //=================================================================// + // Generate functions code (will replace the $(ALLOCATIONCODE) tag) // + //=================================================================// + //Generate function description comment + matlabAllocationCode.append("% " + objectName + " typecasting\n"); + QString allocationFields; + + //Add timestamp + allocationFields.append("\t" + objectName + ".timestamp = " + + "double(typecast(buffer(mcolon(" + objectName + "FidIdx " + "- 20, " + objectName + "FidIdx + 4-1 -20)), 'uint32'))';\n"); + + int currentIdx=0; + + //Add Instance ID, if necessary + if(!info->isSingleInst){ + allocationFields.append("\t" + objectName + ".instanceID = " + + "double(typecast(buffer(mcolon(" + objectName + "FidIdx " + ", " + objectName + "FidIdx + 2-1)), 'uint16'))';\n"); + currentIdx+=2; + } + + for (int n = 0; n < info->fields.length(); ++n) { + // Determine variabel type + type = fieldTypeStrMatlab[info->fields[n]->type]; + + //Determine variable type length + QString size = fieldSizeStrMatlab[info->fields[n]->type]; + // Append field + if ( info->fields[n]->numElements > 1 ){ + allocationFields.append("\t" + objectName + "." + info->fields[n]->name + " = " + + "reshape(double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + + ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt()*info->fields[n]->numElements - 1) + ")), '" + type + "')), "+ QString::number(info->fields[n]->numElements, 10) + ", [] );\n"); + } + else{ + allocationFields.append("\t" + objectName + "." + info->fields[n]->name + " = " + + "double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + + ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() - 1) + ")), '" + type + "'))';\n"); + } + currentIdx+=size.toInt()*info->fields[n]->numElements; + } + matlabAllocationCode.append(allocationFields); + matlabAllocationCode.append("\n"); + + //========================================================================// // Generate objects saving code (will replace the $(SAVEOBJECTSCODE) tag) // //========================================================================// @@ -177,103 +189,8 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) //==========================================================================// matlabExportCsvCode.append("\tOPLog2csv(" + objectTableName + ", '"+objectTableName+"', logfile);\n"); // OPLog2csv(ActuatorCommand, 'ActuatorCommand', logfile) - - //=================================================================// - // Generate functions code (will replace the $(FUNCTIONSCODE) tag) // - //=================================================================// - //Generate function description comment - if(0){ - matlabFunctionsCode.append("%%\n% " + objectName + " read function\n"); - - matlabFunctionsCode.append("function [" + objectName + "] = " + functionCall + objectTableName + ", " + tableIdxName + ")" + "\n"); - matlabFunctionsCode.append("\t" + objectName + ".timestamp(" + tableIdxName + ")= timestamp;\n"); - matlabFunctionsCode.append("\tif " + isSingleInst + "\n"); - matlabFunctionsCode.append("\t\theaderSize = 8;\n"); - matlabFunctionsCode.append("\telse\n"); - matlabFunctionsCode.append("\t\t" + objectName + ".instanceID(" + tableIdxName + ") = (fread(fid, 1, 'uint16'));\n"); - matlabFunctionsCode.append("\t\theaderSize = 10;\n"); - matlabFunctionsCode.append("\tend\n\n"); - - // Generate functions code, actual fields of the object - QString funcfields; - - matlabFunctionsCode.append("\tstartPos = ftell(fid) - headerSize;\n"); - - for (int n = 0; n < info->fields.length(); ++n) { - // Determine type - type = fieldTypeStrMatlab[info->fields[n]->type]; - // Append field - if ( info->fields[n]->numElements > 1 ) - funcfields.append("\t" + objectName + "." + info->fields[n]->name + "(:," + tableIdxName + ") = double(fread(fid, " + QString::number(info->fields[n]->numElements, 10) + ", '" + type + "'));\n"); - else - funcfields.append("\t" + objectName + "." + info->fields[n]->name + "(" + tableIdxName + ") = double(fread(fid, 1, '" + type + "'));\n"); - } - matlabFunctionsCode.append(funcfields); - - matlabFunctionsCode.append("\tobjLen = ftell(fid) - startPos;\n"); - // matlabFunctionsCode.append(QString("\tobjLen - %1 -headerSize\n").arg(numBytes)); - - matlabFunctionsCode.append("\t% read CRC\n"); - matlabFunctionsCode.append("\tcrc_read = fread(fid, 1, '*uint8');\n"); - - matlabFunctionsCode.append("\tif checkCRC\n"); - matlabFunctionsCode.append("\t\tfseek(fid, startPos, 'bof');\n"); - matlabFunctionsCode.append("\t\tcrc_calc = compute_crc(uint8(fread(fid,objLen,'uint8')));\n"); - matlabFunctionsCode.append("\t\tfread(fid,1,'uint8');\n"); - matlabFunctionsCode.append("\t\tif (crc_calc ~= crc_read)\n"); - matlabFunctionsCode.append("\t\t\tdisp('CRC Error')\n"); - matlabFunctionsCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName + " - 1;\n"); - matlabFunctionsCode.append("\t\tend\n"); - matlabFunctionsCode.append("\tend\n"); - - matlabFunctionsCode.append("\n\n"); - } - else{ - matlabFunctionsCode.append("% " + objectName + " typecasting\n"); - QString funcfields; - - //Add timestamp - funcfields.append("\t" + objectName + ".timestamp = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx " - "- 20, " + objectName + "FidIdx + 4-1 -20)), 'uint32'))';\n"); - - int currentIdx=0; - - //Add instance, if necessary - if(!info->isSingleInst){ - funcfields.append("\t" + objectName + ".instanceID = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx " - ", " + objectName + "FidIdx + 2-1)), 'uint16'))';\n"); - currentIdx+=2; - } - - for (int n = 0; n < info->fields.length(); ++n) { - // Determine type - type = fieldTypeStrMatlab[info->fields[n]->type]; - - //Determine variable length - QString size = fieldSizeStrMatlab[info->fields[n]->type]; - // Append field - if ( info->fields[n]->numElements > 1 ){ - funcfields.append("\t" + objectName + "." + info->fields[n]->name + " = " + - "reshape(double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + - ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt()*info->fields[n]->numElements - 1) + ")), '" + type + "')), "+ QString::number(info->fields[n]->numElements, 10) + ", [] );\n"); - } - else{ - funcfields.append("\t" + objectName + "." + info->fields[n]->name + " = " + - "double(typecast(buffer(mcolon(" + objectName + "FidIdx + " + QString("%1").arg(currentIdx) + - ", " + objectName + "FidIdx + " + QString("%1").arg(currentIdx + size.toInt() - 1) + ")), '" + type + "'))';\n"); - } - currentIdx+=size.toInt()*info->fields[n]->numElements; - } - matlabFunctionsCode.append(funcfields); - matlabFunctionsCode.append("\n"); - - } - -// ActuatorSettings.ChannelUpdateFreq(:,actuatorsettingsIdx) = double(fread(fid, 4, 'uint16')); return true; } diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h index afeaf7410..67817867b 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.h @@ -36,12 +36,12 @@ public: private: bool process_object(ObjectInfo* info, int numBytes); - QString matlabAllocationCode; + QString matlabInstantiationCode; QString matlabSwitchCode; QString matlabCleanupCode; + QString matlabAllocationCode; QString matlabSaveObjectsCode; QString matlabExportCsvCode; - QString matlabFunctionsCode; QStringList fieldTypeStrMatlab; QStringList fieldSizeStrMatlab; From f23eabf469bf84fd87dcb3baae02e99bc63bb82c Mon Sep 17 00:00:00 2001 From: Laura Sebesta Date: Thu, 16 Aug 2012 08:47:45 +0200 Subject: [PATCH 03/12] Fixed comments to mention "instantiation" instead of "allocation". --- .../matlab/uavobjectgeneratormatlab.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp index 97b89ad80..515366112 100644 --- a/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp +++ b/ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp @@ -85,29 +85,29 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo* info, int numBytes) QString numBytesString=QString("%1").arg(numBytes); - //===================================================================// - // Generate allocation code (will replace the $(ALLOCATIONCODE) tag) // - //===================================================================// + //=========================================================================// + // Generate instantiation code (will replace the $(INSTANTIATIONCODE) tag) // + //=========================================================================// QString type; - QString allocfields; + QString instantiationFields; matlabInstantiationCode.append("\n\t" + tableIdxName + " = 0;\n"); matlabInstantiationCode.append("\t" + objectTableName + "=struct('timestamp', 0"); if (!info->isSingleInst) { - allocfields.append(",...\n\t\t 'instanceID', 0"); + instantiationFields.append(",...\n\t\t 'instanceID', 0"); } for (int n = 0; n < info->fields.length(); ++n) { // Determine type type = fieldTypeStrMatlab[info->fields[n]->type]; // Append field if ( info->fields[n]->numElements > 1 ) - allocfields.append(",...\n\t\t '" + info->fields[n]->name + "', zeros(" + QString::number(info->fields[n]->numElements, 10) + ",1)"); + instantiationFields.append(",...\n\t\t '" + info->fields[n]->name + "', zeros(" + QString::number(info->fields[n]->numElements, 10) + ",1)"); else - allocfields.append(",...\n\t\t '" + info->fields[n]->name + "', 0"); + instantiationFields.append(",...\n\t\t '" + info->fields[n]->name + "', 0"); } - allocfields.append(");\n"); + instantiationFields.append(");\n"); - matlabInstantiationCode.append(allocfields); + matlabInstantiationCode.append(instantiationFields); matlabInstantiationCode.append("\t" + objectTableName.toUpper() + "_OBJID=" + objectID + ";\n"); matlabInstantiationCode.append("\t" + objectTableName.toUpper() + "_NUMBYTES=" + numBytesString + ";\n"); matlabInstantiationCode.append("\t" + objectName + "FidIdx = [];\n"); From a94142ec300c9c7477e57d2040ad94c25d39834a Mon Sep 17 00:00:00 2001 From: Laura Sebesta Date: Sat, 18 Aug 2012 14:18:10 +0200 Subject: [PATCH 04/12] Fixed several compile warning complaints. Conflicts: ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/uavitem.cpp --- .../src/libs/opmapcontrol/src/mapwidget/homeitem.cpp | 2 +- .../src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp | 6 ++++-- .../config/cfg_vehicletypes/configfixedwingwidget.cpp | 6 +++--- .../config/cfg_vehicletypes/configgroundvehiclewidget.cpp | 8 ++++---- .../config/cfg_vehicletypes/configmultirotorwidget.cpp | 2 +- .../src/plugins/config/configoutputwidget.cpp | 2 +- .../openpilotgcs/src/plugins/config/configrevowidget.cpp | 4 ++-- .../src/plugins/config/configvehicletypewidget.cpp | 2 +- .../src/plugins/gpsdisplay/telemetryparser.cpp | 2 +- .../src/plugins/ipconnection/ipconnectionplugin.cpp | 2 +- ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp | 1 + .../openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp | 2 +- .../openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp | 2 +- ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp | 6 +++--- .../src/plugins/uavobjectutil/uavobjectutilmanager.cpp | 4 ++-- .../src/plugins/uavobjectwidgetutils/configtaskwidget.cpp | 2 +- ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp | 2 +- ground/openpilotgcs/src/plugins/uploader/SSP/port.h | 2 +- 18 files changed, 30 insertions(+), 27 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp index 6dad1e034..09ef9d497 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/homeitem.cpp @@ -28,7 +28,7 @@ namespace mapcontrol { HomeItem::HomeItem(MapGraphicItem* map,OPMapWidget* parent):safe(true),map(map),mapwidget(parent), - showsafearea(true),safearea(1000),altitude(0),isDragging(false),toggleRefresh(true) + showsafearea(true),toggleRefresh(true),safearea(1000),altitude(0),isDragging(false) { pic.load(QString::fromUtf8(":/markers/images/home2.svg")); pic=pic.scaled(30,30,Qt::IgnoreAspectRatio); diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp index 30eea862d..2091f5eaf 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp @@ -34,7 +34,7 @@ namespace mapcontrol { OPMapWidget::OPMapWidget(QWidget *parent, Configuration *config):QGraphicsView(parent),configuration(config),UAV(0),GPS(0),Home(0) - ,followmouse(true),compass(0),showuav(false),showhome(false),showDiag(false),diagGraphItem(0),diagTimer(0),overlayOpacity(1) + ,followmouse(true),compass(0),showuav(false),showhome(false),diagTimer(0),diagGraphItem(0),showDiag(false),overlayOpacity(1) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred); core=new internals::Core; @@ -392,7 +392,7 @@ namespace mapcontrol } void OPMapWidget::WPDeleteAll() { - int x=0; +// int x=0; foreach(QGraphicsItem* i,map->childItems()) { WayPointItem* w=qgraphicsitem_cast(i); @@ -419,7 +419,9 @@ namespace mapcontrol } } } + return false; } + void OPMapWidget::deleteAllOverlays() { foreach(QGraphicsItem* i,map->childItems()) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 7ed667828..0a2064168 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -291,7 +291,7 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) int channel; //disable all - for (channel=0; channelsetupUi(this); diff --git a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp index 1a765a381..53e949c1e 100644 --- a/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configvehicletypewidget.cpp @@ -109,7 +109,7 @@ ConfigVehicleTypeWidget::ConfigVehicleTypeWidget(QWidget *parent) : ConfigTaskWi //Generate lists of mixerTypeNames, mixerVectorNames, channelNames channelNames << "None"; - for (int i = 0; i < ActuatorSettings::CHANNELADDR_NUMELEM; i++) { + for (unsigned int i = 0; i < ActuatorSettings::CHANNELADDR_NUMELEM; i++) { mixerTypes << QString("Mixer%1Type").arg(i+1); mixerVectors << QString("Mixer%1Vector").arg(i+1); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp index e08d2be52..b0db7f291 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp @@ -117,7 +117,7 @@ void TelemetryParser::updateSats( UAVObject* object1) { UAVObjectField* azimuth = object1->getField(QString("Azimuth")); UAVObjectField* snr = object1->getField(QString("SNR")); - for (int i=0;i< prn->getNumElements();i++) { + for (unsigned int i=0;i< prn->getNumElements();i++) { emit satellite(i,prn->getValue(i).toInt(),elevation->getValue(i).toInt(), azimuth->getValue(i).toInt(), snr->getValue(i).toInt()); } diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp index 5d166584c..78a056378 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp @@ -73,7 +73,7 @@ void IPConnection::onOpenDevice(QString HostName, int Port, bool UseTCP) { QAbstractSocket *ipSocket; const int Timeout = 5 * 1000; - int state; +// int state; ipConMutex.lock(); if (UseTCP) { diff --git a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp index 04bcdd93c..4bf2f664e 100644 --- a/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/flightdatamodel.cpp @@ -169,6 +169,7 @@ bool flightDataModel::setColumnByIndex(pathPlanData *row,const int index,const default: return false; } + return false; } QVariant flightDataModel::getColumnByIndex(const pathPlanData *row,const int index) const { diff --git a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp index b5fb5b273..2d8e01506 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmapgadgetwidget.cpp @@ -1773,7 +1773,7 @@ void OPMapGadgetWidget::onAddWayPointAct_triggered(internals::PointLatLng coord) if (m_map_mode != Normal_MapMode) return; - float alt=15; + mapProxy->createWayPoint(coord); } diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp index 382c43829..b490f5ddc 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp @@ -315,7 +315,7 @@ int pjrc_rawhid::send(int num, void *buf, int len, int timeout) QString pjrc_rawhid::getserial(int num) { hid_t *hid; - char buf[128]; +// char buf[128]; hid = get_hid(num); diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp index 51fd6cce7..3e661c598 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp @@ -45,8 +45,8 @@ static bool HID_GetStrProperty(IOHIDDeviceRef dev, CFStringRef property, QString */ USBMonitor::USBMonitor(QObject *parent): QThread(parent) { hid_manager=NULL; - CFMutableDictionaryRef dict; - CFNumberRef num; +// CFMutableDictionaryRef dict; +// CFNumberRef num; IOReturn ret; m_instance = this; @@ -131,7 +131,7 @@ void USBMonitor::attach_callback(void *context, IOReturn r, void *hid_mgr, IOHID { bool got_properties = true; - CFTypeRef prop; +// CFTypeRef prop; USBPortInfo deviceInfo; deviceInfo.dev_handle = dev; diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index be81befb7..1c69c63e9 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -268,7 +268,7 @@ QByteArray UAVObjectUtilManager::getBoardCPUSerial() QByteArray cpuSerial; FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); - for (int i = 0; i < FirmwareIAPObj::CPUSERIAL_NUMELEM; i++) + for (unsigned int i = 0; i < FirmwareIAPObj::CPUSERIAL_NUMELEM; i++) cpuSerial.append(firmwareIapData.CPUSerial[i]); return cpuSerial; @@ -288,7 +288,7 @@ QByteArray UAVObjectUtilManager::getBoardDescription() QByteArray ret; FirmwareIAPObj::DataFields firmwareIapData = getFirmwareIap(); - for (int i = 0; i < FirmwareIAPObj::DESCRIPTION_NUMELEM; i++) + for (unsigned int i = 0; i < FirmwareIAPObj::DESCRIPTION_NUMELEM; i++) ret.append(firmwareIapData.Description[i]); return ret; diff --git a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp index 594063358..1d711c0d8 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp @@ -32,7 +32,7 @@ /** * Constructor */ -ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent),isConnected(false),smartsave(NULL),dirty(false),outOfLimitsStyle("background-color: rgb(255, 0, 0);"),timeOut(NULL),allowWidgetUpdates(true) +ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent),isConnected(false),allowWidgetUpdates(true),smartsave(NULL),dirty(false),outOfLimitsStyle("background-color: rgb(255, 0, 0);"),timeOut(NULL) { pm = ExtensionSystem::PluginManager::instance(); objManager = pm->getObject(); diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index e1aa5ecf6..137fd4dc3 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -457,7 +457,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) default: rxState = STATE_SYNC; stats.rxErrors++; - UAVTALK_QXTLOG_DEBUG("UAVTalk: ???->Sync"); + UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); //Use the escape character for '?' so that the tripgraph isn't triggered. } // Done diff --git a/ground/openpilotgcs/src/plugins/uploader/SSP/port.h b/ground/openpilotgcs/src/plugins/uploader/SSP/port.h index ff4725b99..1521ab412 100644 --- a/ground/openpilotgcs/src/plugins/uploader/SSP/port.h +++ b/ground/openpilotgcs/src/plugins/uploader/SSP/port.h @@ -43,7 +43,7 @@ public: uint8_t maxRetryCount; // max. times to try to transmit the 'send' packet uint16_t max_retry; // Maximum number of retrys for a single transmit. int32_t timeoutLen; // how long to wait for each retry to succeed - int32_t timeout; // current timeout. when 'time' reaches this point we have timed out + uint32_t timeout; // current timeout. when 'time' reaches this point we have timed out uint8_t txSeqNo; // current 'send' packet sequence number uint16_t rxBufPos; // current buffer position in the receive packet uint16_t rxBufLen; // number of 'data' bytes in the buffer From 9ebc32da3df84652296d81280d515bb98378ddd0 Mon Sep 17 00:00:00 2001 From: Kenz Dale Date: Thu, 23 Aug 2012 18:03:17 +0200 Subject: [PATCH 05/12] Eliminated several (dozen) compiler warnings. Conflicts: ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp --- .../src/libs/opmapcontrol/src/core/opmaps.cpp | 2 + .../projections/platecarreeprojection.cpp | 1 + .../platecarreeprojectionpergo.cpp | 1 + .../opmapcontrol/src/mapwidget/trailitem.cpp | 3 + .../src/mapwidget/waypointcircle.cpp | 3 + .../src/mapwidget/waypointline.cpp | 2 + .../src/libs/utils/mytabbedstackwidget.cpp | 2 + .../cfg_vehicletypes/configccpmwidget.cpp | 1 + .../config/cfg_vehicletypes/vehicleconfig.cpp | 2 + .../src/plugins/config/configgadgetwidget.cpp | 2 + .../src/plugins/config/configinputwidget.cpp | 2 +- .../plugins/config/configpipxtremewidget.cpp | 4 +- .../src/plugins/coreplugin/modemanager.cpp | 4 ++ .../plugins/gcscontrol/gcscontrolgadget.cpp | 4 +- .../gcscontrolgadgetoptionspage.cpp | 4 ++ .../gcscontrol/gcscontrolgadgetwidget.cpp | 2 + .../gpsdisplaygadgetoptionspage.cpp | 2 + .../src/plugins/gpsdisplay/nmeaparser.cpp | 2 +- .../ipconnectionconfiguration.cpp | 2 + .../ipconnection/ipconnectionplugin.cpp | 4 ++ .../src/plugins/opmap/modelmapproxy.cpp | 6 ++ .../opmap/opmap_edit_waypoint_dialog.cpp | 2 + .../src/plugins/pfd/pfdgadgetoptionspage.cpp | 1 + .../qmlview/qmlviewgadgetoptionspage.cpp | 1 + .../src/plugins/rawhid/pjrc_rawhid_mac.cpp | 55 +++++++++++++------ .../src/plugins/rawhid/usbmonitor_mac.cpp | 8 +++ .../serialpluginconfiguration.cpp | 2 + .../src/plugins/uploader/SSP/qssp.cpp | 2 + .../uploader/uploadergadgetoptionspage.cpp | 2 + 29 files changed, 108 insertions(+), 20 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp index 86660fd3e..3b858d82f 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/core/opmaps.cpp @@ -43,6 +43,8 @@ namespace core { LanguageStr=LanguageType().toShortString(Language); Cache::Instance(); +// OPMaps::MemoryCache(); + } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp index 18688ca06..27fd90808 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojection.cpp @@ -91,6 +91,7 @@ Size PlateCarreeProjection::GetTileMatrixMaxXY(const int &zoom) Size PlateCarreeProjection::GetTileMatrixMinXY(const int &zoom) { + Q_UNUSED(zoom); return Size(0, 0); } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp index 106674ad2..89d598bd6 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/internals/projections/platecarreeprojectionpergo.cpp @@ -90,6 +90,7 @@ Size PlateCarreeProjectionPergo::GetTileMatrixMaxXY(const int &zoom) Size PlateCarreeProjectionPergo::GetTileMatrixMinXY(const int &zoom) { + Q_UNUSED(zoom) return Size(0, 0); } } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp index fabf6ee9a..ae13de21b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/trailitem.cpp @@ -37,6 +37,9 @@ TrailItem::TrailItem(internals::PointLatLng const& coord,int const& altitude, QB void TrailItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) { + Q_UNUSED(option); + Q_UNUSED(widget); + painter->setBrush(m_brush); painter->drawEllipse(-2,-2,4,4); } diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp index 9f045a701..aad50ef0b 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointcircle.cpp @@ -60,6 +60,9 @@ int WayPointCircle::type() const void WayPointCircle::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) { + Q_UNUSED(option); + Q_UNUSED(widget); + QPointF p1; QPointF p2; p1=QPointF(line.p1().x(),line.p1().y()+line.length()); diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp index 527ca46f9..ad245cbc1 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/waypointline.cpp @@ -75,6 +75,8 @@ QPainterPath WayPointLine::shape() const } void WayPointLine::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) { + Q_UNUSED(option); + Q_UNUSED(widget); QPen myPen = pen(); myPen.setColor(myColor); diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp index c412a475c..7c26acf45 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -114,6 +114,8 @@ void MyTabbedStackWidget::showWidget(int index) void MyTabbedStackWidget::insertCornerWidget(int index, QWidget *widget) { + Q_UNUSED(index); + widget->hide(); } diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp index e7ca51fd1..47b730dbf 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configccpmwidget.cpp @@ -218,6 +218,7 @@ ConfigCcpmWidget::~ConfigCcpmWidget() void ConfigCcpmWidget::setupUI(QString frameType) { + Q_UNUSED(frameType); } void ConfigCcpmWidget::ResetActuators(GUIConfigDataUnion* configData) diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp index fe76f1660..63f4351bb 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/vehicleconfig.cpp @@ -112,7 +112,9 @@ void VehicleConfig::SetConfigData(GUIConfigDataUnion configData) { void VehicleConfig::ResetActuators(GUIConfigDataUnion* configData) { + Q_UNUSED(configData); } + QStringList VehicleConfig::getChannelDescriptions() { QStringList channelDesc; diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index c466df760..b512a2642 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -218,6 +218,8 @@ void ConfigGadgetWidget::tabAboutToChange(int i,bool * proceed) */ void ConfigGadgetWidget::updatePipXStatus(UAVObject *object) { + Q_UNUSED(object); + // Restart the disconnection timer. pipxTimeout->start(5000); if (!pipxConnected) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index e0425d963..7f6c6339f 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -1268,7 +1268,7 @@ void ConfigInputWidget::simpleCalibration(bool enable) restoreMdata(); - for (int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) + for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; // Force flight mode neutral to middle diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 7808b6dfa..7d46840ac 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -266,7 +266,9 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) */ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) { - if (!settingsUpdated) + Q_UNUSED(object); + + if (!settingsUpdated) { settingsUpdated = true; enableControls(true); diff --git a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp index a4479bae2..aa5356705 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/modemanager.cpp @@ -84,6 +84,8 @@ void ModeManager::init() void ModeManager::addWidget(QWidget *widget) { + Q_UNUSED(widget); + // We want the actionbar to stay on the bottom // so m_modeStack->cornerWidgetCount() -1 inserts it at the position immediately above // the actionbar @@ -220,6 +222,8 @@ void ModeManager::aboutToRemoveObject(QObject *obj) void ModeManager::addAction(Command *command, int priority, QMenu *menu) { + Q_UNUSED(menu); + m_actions.insert(command, priority); // Count the number of commands with a higher priority diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index b95082fb9..485cd7b31 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -198,6 +198,8 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r void GCSControlGadget::gamepads(quint8 count) { + Q_UNUSED(count); + // sdlGamepad.setGamepad(0); // sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE); } @@ -291,7 +293,7 @@ double GCSControlGadget::constrain(double value) void GCSControlGadget::buttonState(ButtonNumber number, bool pressed) { - int state; +// int state; if ((buttonSettings[number].ActionID>0)&&(buttonSettings[number].FunctionID>0)&&(pressed)) {//this button is configured ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp index 1a68c6be6..d85390725 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp @@ -71,6 +71,8 @@ void GCSControlGadgetOptionsPage::buttonState(ButtonNumber number, bool pressed) void GCSControlGadgetOptionsPage::gamepads(quint8 count) { + Q_UNUSED(count); + /*options_page->AvailableControllerList->clear(); for (int i=0;igetObject(); UAVDataObject* flightStatus = dynamic_cast( objManager->getObject(QString("FlightStatus")) ); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp index 10d89d26e..e6e9c49d1 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsdisplaygadgetoptionspage.cpp @@ -165,6 +165,8 @@ return s1.portNamesetupUi(optionsPageWidget); diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp index e856251ab..cca79a565 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/nmeaparser.cpp @@ -301,7 +301,7 @@ void NMEAParser::nmeaProcessGPGSV(char *packet) const int sentence_total = tokenslist.at(1).toInt(); // Number of sentences for full data const int sentence_index = tokenslist.at(2).toInt(); // sentence x of y - const int sat_count = tokenslist.at(3).toInt(); // Number of satellites in view +// const int sat_count = tokenslist.at(3).toInt(); // Number of satellites in view int sats = (tokenslist.size() - 4) /4; for(int sat = 0; sat < sats; sat++) { diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp index e90f52ab2..2fabfa8bb 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionconfiguration.cpp @@ -34,6 +34,8 @@ IPconnectionConfiguration::IPconnectionConfiguration(QString classId, QSettings* m_Port(1000), m_UseTCP(1) { + Q_UNUSED(qSettings); + settings = Core::ICore::instance()->settings(); } diff --git a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp index 78a056378..ed0d42d86 100644 --- a/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp +++ b/ground/openpilotgcs/src/plugins/ipconnection/ipconnectionplugin.cpp @@ -177,6 +177,8 @@ QList IPconnectionConnection::availableDevices() QIODevice *IPconnectionConnection::openDevice(const QString &deviceName) { + Q_UNUSED(deviceName); + QString HostName; int Port; bool UseTCP; @@ -211,6 +213,8 @@ QIODevice *IPconnectionConnection::openDevice(const QString &deviceName) void IPconnectionConnection::closeDevice(const QString &deviceName) { + Q_UNUSED(deviceName); + if (ipSocket){ ipConMutex.lock(); emit CloseSocket(ipSocket); diff --git a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp index 406d8e23d..a802d69f8 100644 --- a/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/modelmapproxy.cpp @@ -60,6 +60,8 @@ void modelMapProxy::WPValuesChanged(WayPointItem * wp) void modelMapProxy::currentRowChanged(QModelIndex current, QModelIndex previous) { + Q_UNUSED(previous); + QList list; WayPointItem * wp=findWayPointNumber(current.row()); if(!wp) @@ -202,6 +204,8 @@ WayPointItem * modelMapProxy::findWayPointNumber(int number) void modelMapProxy::rowsRemoved(const QModelIndex &parent, int first, int last) { + Q_UNUSED(parent); + for(int x=last;x>first-1;x--) { myMap->WPDelete(x); @@ -211,6 +215,8 @@ void modelMapProxy::rowsRemoved(const QModelIndex &parent, int first, int last) void modelMapProxy::dataChanged(const QModelIndex &topLeft, const QModelIndex &bottomRight) { + Q_UNUSED(bottomRight); + WayPointItem * item=findWayPointNumber(topLeft.row()); if(!item) return; diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp index cab739b8e..11df41f88 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp @@ -287,5 +287,7 @@ void opmap_edit_waypoint_dialog::enableEditWidgets(bool value) void opmap_edit_waypoint_dialog::currentRowChanged(QModelIndex current, QModelIndex previous) { + Q_UNUSED(previous); + mapper->setCurrentIndex(current.row()); } diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp index 89a877a0f..4268742e9 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetoptionspage.cpp @@ -46,6 +46,7 @@ PFDGadgetOptionsPage::PFDGadgetOptionsPage(PFDGadgetConfiguration *config, QObje //creates options page widget (uses the UI file) QWidget *PFDGadgetOptionsPage::createPage(QWidget *parent) { + Q_UNUSED(parent); options_page = new Ui::PFDGadgetOptionsPage(); //main widget diff --git a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp index 8cc110d1a..f88299cdb 100644 --- a/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/qmlview/qmlviewgadgetoptionspage.cpp @@ -46,6 +46,7 @@ QmlViewGadgetOptionsPage::QmlViewGadgetOptionsPage(QmlViewGadgetConfiguration *c //creates options page widget (uses the UI file) QWidget *QmlViewGadgetOptionsPage::createPage(QWidget *parent) { + Q_UNUSED(parent); options_page = new Ui::QmlViewGadgetOptionsPage(); //main widget diff --git a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp index b490f5ddc..490ea2fdd 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/pjrc_rawhid_mac.cpp @@ -90,7 +90,7 @@ static void hid_close(hid_t *); static void attach_callback(void *, IOReturn, void *, IOHIDDeviceRef); static void detach_callback(void *, IOReturn, void *hid_mgr, IOHIDDeviceRef dev); static void input_callback(void *, IOReturn, void *, IOHIDReportType, uint32_t, uint8_t *, CFIndex); -static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len); +//static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len); static void timeout_callback(CFRunLoopTimerRef, void *); @@ -173,7 +173,7 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) IOHIDManagerRegisterDeviceRemovalCallback(hid_manager, detach_callback, NULL); ret = IOHIDManagerOpen(hid_manager, kIOHIDOptionsTypeNone); if (ret != kIOReturnSuccess) { - printf("Could not start IOHIDManager"); + qDebug()<< "Could not start IOHIDManager"; IOHIDManagerUnscheduleFromRunLoop(hid_manager, CFRunLoopGetCurrent(), kCFRunLoopDefaultMode); CFRelease(hid_manager); @@ -181,7 +181,7 @@ int pjrc_rawhid::open(int max, int vid, int pid, int usage_page, int usage) } // Set the run loop reference: the_correct_runloop = CFRunLoopGetCurrent(); - printf("run loop\n"); + qDebug() << "run loop"; // let it do the callback for all devices while (CFRunLoopRunInMode(kCFRunLoopDefaultMode, 0, true) == kCFRunLoopRunHandledSource) ; // count up how many were added by the callback @@ -240,7 +240,7 @@ int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) break; } if (!hid->open) { - printf("pjrc_rawhid_recv, device not open\n"); + qDebug() << "pjrc_rawhid_recv, device not open\n"; ret = -1; break; } @@ -263,6 +263,8 @@ int pjrc_rawhid::receive(int num, void *buf, int len, int timeout) // int pjrc_rawhid::send(int num, void *buf, int len, int timeout) { + Q_UNUSED(timeout); + hid_t *hid; int result=-100; @@ -287,6 +289,7 @@ int pjrc_rawhid::send(int num, void *buf, int len, int timeout) #endif #if 0 +#define TIMEOUT_FIXED // No matter what I tried this never actually sends an output // report and output_callback never gets called. Why?? // Did I miss something? This is exactly the same params as @@ -358,6 +361,9 @@ void pjrc_rawhid::close(int num) // static void input_callback(void *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) { + Q_UNUSED(type); + Q_UNUSED(id); + buffer_t *n; hid_t *hid; @@ -365,7 +371,7 @@ static void input_callback(void *context, IOReturn ret, void *sender, IOHIDRepor if (ret != kIOReturnSuccess || len < 1) return; hid = (hid_t*)context; if (!hid || hid->ref != sender) return; - printf("Processing packet"); + qDebug() << "Processing packet"; n = (buffer_t *)malloc(sizeof(buffer_t)); if (!n) return; if (len > BUFFER_SIZE) len = BUFFER_SIZE; @@ -385,6 +391,8 @@ static void input_callback(void *context, IOReturn ret, void *sender, IOHIDRepor static void timeout_callback(CFRunLoopTimerRef timer, void *info) { + Q_UNUSED(timer); + //qDebug("timeout_callback\n"); *(int *)info = 1; //qDebug() << "Stop CFRunLoop from timeout_callback" << CFRunLoopGetCurrent(); @@ -440,9 +448,13 @@ static void hid_close(hid_t *hid) static void detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { + Q_UNUSED(context); + Q_UNUSED(r); + Q_UNUSED(hid_mgr); + hid_t *p; - printf("detach callback\n"); + qDebug()<< "detach callback"; for (p = first_hid; p; p = p->next) { if (p->ref == dev) { p->open = 0; @@ -454,9 +466,13 @@ static void detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDevic static void attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { + Q_UNUSED(context); + Q_UNUSED(r); + Q_UNUSED(hid_mgr); + struct hid_struct *h; - printf("attach callback\n"); + qDebug() << "attach callback"; if (IOHIDDeviceOpen(dev, kIOHIDOptionsTypeNone) != kIOReturnSuccess) return; h = (hid_t *)malloc(sizeof(hid_t)); if (!h) return; @@ -468,15 +484,22 @@ static void attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDevic add_hid(h); } +#ifdef TIMEOUT_FIXED static void output_callback(hid_t *context, IOReturn ret, void *sender, IOHIDReportType type, uint32_t id, uint8_t *data, CFIndex len) { - printf("output_callback, r=%d\n", ret); - if (ret == kIOReturnSuccess) { - *(int *)context = len; - } else { - // timeout if not success? - *(int *)context = 0; - } - CFRunLoopStop(CFRunLoopGetCurrent()); -} + Q_UNUSED(sender); + Q_UNUSED(type); + Q_UNUSED(id); + Q_UNUSED(data); + qDebug()<< QString("output_callback, r=%1").arg(ret); +// printf("output_callback, r=%d\n", ret); + if (ret == kIOReturnSuccess) { + *(int *)context = len; + } else { + // timeout if not success? + *(int *)context = 0; + } + CFRunLoopStop(CFRunLoopGetCurrent()); +} +#endif diff --git a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp index 3e661c598..d803512e4 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp @@ -117,6 +117,10 @@ void USBMonitor::removeDevice(IOHIDDeviceRef dev) { */ void USBMonitor::detach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { + Q_UNUSED(context); + Q_UNUSED(r); + Q_UNUSED(hid_mgr); + qDebug() << "USBMonitor: Device detached event"; instance()->removeDevice(dev); } @@ -129,6 +133,10 @@ void USBMonitor::addDevice(USBPortInfo info) { void USBMonitor::attach_callback(void *context, IOReturn r, void *hid_mgr, IOHIDDeviceRef dev) { + Q_UNUSED(context); + Q_UNUSED(r); + Q_UNUSED(hid_mgr); + bool got_properties = true; // CFTypeRef prop; diff --git a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp index 789085596..75510f52c 100644 --- a/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/serialconnection/serialpluginconfiguration.cpp @@ -38,6 +38,8 @@ SerialPluginConfiguration::SerialPluginConfiguration(QString classId, QSettings* IUAVGadgetConfiguration(classId, parent), m_speed("57600") { + Q_UNUSED(qSettings); + settings = Core::ICore::instance()->settings(); } diff --git a/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp b/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp index 356cb3604..b5eb5b1b7 100644 --- a/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/SSP/qssp.cpp @@ -802,6 +802,8 @@ qssp::qssp(port * info,bool debug):debug(debug) } void qssp::pfCallBack( uint8_t * buf, uint16_t size) { + Q_UNUSED(size); + if (debug) qDebug()<<"receive callback"< Date: Sat, 18 Aug 2012 16:53:41 +0200 Subject: [PATCH 06/12] Redefined `isIntermediateValueHelper` as class static. --- ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp | 2 +- ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp index 7192aabe1..10b0db0bc 100644 --- a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp +++ b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.cpp @@ -492,7 +492,7 @@ QString QScienceSpinBox::stripped(const QString &t, int *pos) const } // reimplemented function, copied from qspinbox.cpp -static bool isIntermediateValueHelper(qint64 num, qint64 min, qint64 max, qint64 *match) +bool QScienceSpinBox::isIntermediateValueHelper(qint64 num, qint64 min, qint64 max, qint64 *match) { QSBDEBUG("%lld %lld %lld", num, min, max); diff --git a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h index 812447840..8b2ab98ed 100644 --- a/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h +++ b/ground/openpilotgcs/src/libs/qscispinbox/QScienceSpinBox.h @@ -11,7 +11,6 @@ #include #include -static bool isIntermediateValueHelper(qint64 num, qint64 minimum, qint64 maximum, qint64 *match = 0); class QScienceSpinBox : public QDoubleSpinBox { @@ -24,6 +23,7 @@ public: QString textFromValue ( double value ) const; double valueFromText ( const QString & text ) const; + static bool isIntermediateValueHelper(qint64 num, qint64 minimum, qint64 maximum, qint64 *match = 0); private: int dispDecimals; From 0ff46af74cf86ae814cea04aeb6b4041d08bc8ca Mon Sep 17 00:00:00 2001 From: Kenz Dale Date: Thu, 23 Aug 2012 18:10:27 +0200 Subject: [PATCH 07/12] Changes requested in review. All cosmetic (removing comments or fixing whitespaces), so does not affect functionality. --- .../src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp | 1 - .../config/cfg_vehicletypes/configfixedwingwidget.cpp | 6 +++--- .../config/cfg_vehicletypes/configgroundvehiclewidget.cpp | 6 +++--- .../config/cfg_vehicletypes/configmultirotorwidget.cpp | 2 +- .../src/plugins/ipconnection/ipconnectionplugin.cpp | 1 - ground/openpilotgcs/src/plugins/rawhid/usbmonitor_mac.cpp | 3 --- 6 files changed, 7 insertions(+), 12 deletions(-) diff --git a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp index 2091f5eaf..bdc60225d 100644 --- a/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp +++ b/ground/openpilotgcs/src/libs/opmapcontrol/src/mapwidget/opmapwidget.cpp @@ -392,7 +392,6 @@ namespace mapcontrol } void OPMapWidget::WPDeleteAll() { -// int x=0; foreach(QGraphicsItem* i,map->childItems()) { WayPointItem* w=qgraphicsitem_cast(i); diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp index 0a2064168..9d5fec352 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configfixedwingwidget.cpp @@ -291,7 +291,7 @@ bool ConfigFixedWingWidget::setupFrameFixedWing(QString airframeType) int channel; //disable all - for (channel=0; (unsigned int) channel Date: Thu, 30 Aug 2012 12:10:17 -0500 Subject: [PATCH 08/12] AndroidGCS Map: Make sure to grab the correct uavLocation and homeLocation at startup --- androidgcs/src/org/openpilot/androidgcs/UAVLocation.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/androidgcs/src/org/openpilot/androidgcs/UAVLocation.java b/androidgcs/src/org/openpilot/androidgcs/UAVLocation.java index ffb3e427c..6ee1bdbcd 100644 --- a/androidgcs/src/org/openpilot/androidgcs/UAVLocation.java +++ b/androidgcs/src/org/openpilot/androidgcs/UAVLocation.java @@ -169,9 +169,11 @@ public class UAVLocation extends MapActivity void onOPConnected() { UAVObject obj = objMngr.getObject("HomeLocation"); registerObjectUpdates(obj); + objectUpdated(obj); obj = objMngr.getObject("PositionActual"); registerObjectUpdates(obj); + objectUpdated(obj); } private GeoPoint getUavLocation() { From 42a11788f97f9e2adb58606a81abef2b2933191c Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 30 Aug 2012 12:13:29 -0500 Subject: [PATCH 09/12] AndroidGCS Controller: Make the default value mode 2 Previously typed "Mode 2" but it wants the integer value for the default setting. This stops a crash when no configuration. --- androidgcs/res/xml/controller_preferences.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/androidgcs/res/xml/controller_preferences.xml b/androidgcs/res/xml/controller_preferences.xml index 48f263a1b..219a5146e 100644 --- a/androidgcs/res/xml/controller_preferences.xml +++ b/androidgcs/res/xml/controller_preferences.xml @@ -3,7 +3,7 @@ From 803e3c4d1e6e059b6a887887f658d42fde112405 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Wed, 5 Sep 2012 21:41:18 -0400 Subject: [PATCH 10/12] ef: Add fwinfo to entireflash (EF) images EF images were missing the firmware info blob that describes which firmware is installed on the board. The EF image is now padded out to fill the firmware bank and the info blob is properly placed at the tail end of the firmware bank. --- flight/EntireFlash/Makefile | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/flight/EntireFlash/Makefile b/flight/EntireFlash/Makefile index 548461a24..13aae597d 100644 --- a/flight/EntireFlash/Makefile +++ b/flight/EntireFlash/Makefile @@ -33,16 +33,23 @@ OUTDIR := $(TOP)/build/$(TARGET) .PHONY: bin bin: $(OUTDIR)/$(TARGET).bin -FW_PAD = $(shell echo $$[$(FW_BANK_BASE)-$(BL_BANK_BASE)-$(BL_BANK_SIZE)]) -$(OUTDIR)/$(TARGET).pad: +FW_PRE_PAD = $(shell echo $$[$(FW_BANK_BASE)-$(BL_BANK_BASE)-$(BL_BANK_SIZE)]) +$(OUTDIR)/$(TARGET).fw_pre.pad: $(V0) @echo $(MSG_PADDING) $@ - $(V1) dd status=noxfer if=/dev/zero count=$(FW_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@ + $(V1) dd status=noxfer if=/dev/zero count=$(FW_PRE_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@ + +FW_POST_PAD = $(shell echo $$[$(FW_BANK_SIZE)-$(FW_DESC_SIZE)-$$(stat -c%s $(FW_BIN))]) +$(OUTDIR)/$(TARGET).fw_post.pad: + $(V0) @echo $(MSG_PADDING) $@ + $(V1) dd status=noxfer if=/dev/zero count=$(FW_POST_PAD) bs=1 2>/dev/null | tr '\000' '\377' > $@ + BL_BIN = $(TOP)/build/bl_$(BOARD_NAME)/bl_$(BOARD_NAME).bin FW_BIN = $(TOP)/build/fw_$(BOARD_NAME)/fw_$(BOARD_NAME).bin -$(OUTDIR)/$(TARGET).bin: $(BL_BIN) $(FW_BIN) $(OUTDIR)/$(TARGET).pad +FWINFO_BIN = $(FW_BIN).firmwareinfo.bin +$(OUTDIR)/$(TARGET).bin: $(BL_BIN) $(FW_BIN) $(OUTDIR)/$(TARGET).fw_pre.pad $(OUTDIR)/$(TARGET).fw_post.pad $(V0) @echo $(MSG_FLASH_IMG) $@ - $(V1) cat $(BL_BIN) $(OUTDIR)/$(TARGET).pad $(FW_BIN) > $@ + $(V1) cat $(BL_BIN) $(OUTDIR)/$(TARGET).fw_pre.pad $(FW_BIN) $(OUTDIR)/$(TARGET).fw_post.pad $(FWINFO_BIN) > $@ .PHONY: dfu dfu: $(OUTDIR)/$(TARGET).bin From 97882dbef918db2016c08a0ecc38451509486f28 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Wed, 5 Sep 2012 22:17:04 -0400 Subject: [PATCH 11/12] pipx: ensure pipx BL image is padded to fill its bank This padding is required in order to properly align all of the sections in the EF images. --- flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_BL_sections.ld | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_BL_sections.ld b/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_BL_sections.ld index 777e09aa6..19f70959d 100644 --- a/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_BL_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_BL_sections.ld @@ -75,6 +75,7 @@ SECTIONS { . = ALIGN(4); KEEP(*(.boardinfo)) + . = ALIGN(ORIGIN(BD_INFO)+LENGTH(BD_INFO)); } > BD_INFO /* Stabs debugging sections. */ From 4803dfe99c704edef1162d66d9ad33bd964b0b83 Mon Sep 17 00:00:00 2001 From: Werner Backes Date: Thu, 6 Sep 2012 13:13:58 +0200 Subject: [PATCH 12/12] Fix: yaw bias correction wasn't applied on CC3D. --- flight/Modules/Attitude/attitude.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index baa0c5453..6b0c16cec 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -420,6 +420,10 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData) gyrosData->z += gyro_correct_int[2]; } + // Because most crafts wont get enough information from gravity to zero yaw gyro, we try + // and make it average zero (weakly) + gyro_correct_int[2] += - gyrosData->z * yawBiasRate; + GyrosSet(gyrosData); AccelsSet(accelsData);