rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1291 fix Matlab/Octave converter after UAVTalk changes.

Browse Source 
Also made several optimization.
This commit is contained in:
Alessio Morale 2014-04-05 13:00:55 +02:00
parent 8bb1d0148c
commit 6e0f478f43
2 changed files with 48 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
ground/openpilotgcs/src/plugins/uavobjects/uavobject.m.template
View File

@ -59,59 +59,78 @@ $(INSTANTIATIONCODE)
fid = fopen(logfile);
buffer=fread(fid,Inf,'uchar=>uchar');
buffer=fread(fid,Inf,'uchar=>uint8');
fseek(fid, 0, 'bof');
bufferIdx=1;
bufferlen = size(buffer);
correctMsgByte=hex2dec('20');
correctTimestampedByte=hex2dec('A0');
correctSyncByte=hex2dec('3C');
unknownObjIDList=zeros(1,2);
% Parse log file, entry by entry
% prebuf = buffer(1:12);
headerLen = 1 + 1 + 2 + 4 + 2; % sync type len id inst
timestampLen = 4;
crcLen = 1;
oplHeaderLen = 8 + 4;
last_print = -1e10;
bufferIdx=1;
headerIdx=oplHeaderLen + 1;
startTime=clock;
while (1)
if (feof(fid)); break; end
try
%% Read message header
% get sync field (0x3C, 1 byte)
sync = fread(fid, 1, 'uint8');
if (bufferlen < headerIdx + 12); break; end
sync = buffer(headerIdx);
% printf('%x ', sync);
headerIdx += 1;
if sync ~= correctSyncByte
prebuf = [prebuf(2:end); sync];
wrongSyncByte = wrongSyncByte + 1;
continue
end
% get msg type (quint8 1 byte ) should be 0x20, ignore the rest?
msgType = fread(fid, 1, 'uint8');
if msgType ~= correctMsgByte && msgType ~= hex2dec('A0')
wrongMessageByte = wrongMessageByte + 1;
% printf('\n %u:',headerIdx - 1);
% get the opl timestamp and datablock size
oplTimestamp = typecast(uint8(buffer(headerIdx - 1 - 8 - 4:headerIdx - 1 - 8 - 1)), 'uint32');
oplSize = typecast(uint8(buffer(headerIdx - 1 - 8:headerIdx - 1 - 1)), 'uint64');
% get msg type (quint8 1 byte ) should be 0x20/0xA0, ignore the rest
msgType = buffer(headerIdx);
headerIdx += 1;
if msgType ~= correctMsgByte && msgType ~= correctTimestampedByte
% fixme: it should read the whole message payload instead of skipping and blindly searching for next sync byte.
fprintf('\nSkipping message type: %x \n', msgType);
continue
end
% get msg size (quint16 2 bytes) excludes crc, include msg header and data payload
msgSize = fread(fid, 1, 'uint16');
msgSize = uint32(typecast(buffer(headerIdx:headerIdx + 1), 'uint16'));
headerIdx += 2;
% get obj id (quint32 4 bytes)
objID = fread(fid, 1, 'uint32');
objID = typecast(uint8(buffer(headerIdx:headerIdx + 3)), 'uint32');
headerIdx += 4;
% get instance id (quint16 2 bytes)
instID = typecast(uint8(buffer(headerIdx:headerIdx + 1)), 'uint16');
% printf('Id %x type %x size %u Inst %x ', objID, msgType, msgSize, instID);
headerIdx += 2;
% get timestamp if needed (quint32 4 bytes)
if msgType == correctMsgByte
%% Process header if we are aligned
timestamp = typecast(uint8(prebuf(1:4)), 'uint32');
datasize = typecast(uint8(prebuf(5:12)), 'uint64');
datasize = msgSize - headerLen;
elseif msgType == correctTimestampedByte
timestamp = fread(fid,1,'uint16');
end
if (isempty(objID)) %End of file
break;
timestamp = typecast(uint8(buffer(headerIdx:headerIdx + 3)), 'uint32');
% printf('ts %u');
headerIdx += 4;
datasize = msgSize - headerLen - timestampLen;
end
% printf('\n');
bufferIdx = headerIdx;
headerIdx += datasize + crcLen + oplHeaderLen;
%% Read object
@ -125,9 +144,7 @@ $(SWITCHCODE)
unknownObjIDList(unknownObjIDListIdx,2)=unknownObjIDList(unknownObjIDListIdx,2)+1;
end
datasize = typecast(buffer(datasizeBufferIdx + 4:datasizeBufferIdx + 12-1), 'uint64');
msgBytesLeft = datasize - 1 - 1 - 2 - 4;
msgBytesLeft = msgSize - 1 - 1 - 2 - 4;
if msgBytesLeft > 255
msgBytesLeft = 0;
end

10
ground/uavobjgenerator/generators/matlab/uavobjectgeneratormatlab.cpp
View File

@ -122,9 +122,6 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo *info, int numBytes)
matlabSwitchCode.append("\t\t\t" + tableIdxName + " = " + tableIdxName + " + 1;\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");
}
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");
@ -145,16 +142,15 @@ bool UAVObjectGeneratorMatlab::process_object(ObjectInfo *info, int numBytes)
// Add timestamp
allocationFields.append("\t" + objectName + ".timestamp = " +
"double(typecast(buffer(mcolon(" + objectName + "FidIdx "
"- 20, " + objectName + "FidIdx + 4-1 -20)), 'uint32'))';\n");
"- headerLen - oplHeaderLen, " + objectName + "FidIdx + 3 - headerLen - oplHeaderLen)), '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;
"double(typecast(buffer(mcolon(" + objectName + "FidIdx - 2"
", " + objectName + "FidIdx - 2 + 1)), 'uint16'))';\n");
}
for (int n = 0; n < info->fields.length(); ++n) {