void swap_ape_header(APE_HEADER *header)
{
header->nCompressionLevel = swap_int16(header->nCompressionLevel);
header->nFormatFlags = swap_int16(header->nFormatFlags);
header->nBlocksPerFrame = swap_int32(header->nBlocksPerFrame);
header->nFinalFrameBlocks = swap_int32(header->nFinalFrameBlocks);
header->nTotalFrames = swap_int32(header->nTotalFrames);
header->nBitsPerSample = swap_int16(header->nBitsPerSample);
header->nChannels = swap_int16(header->nChannels);
header->nSampleRate = swap_int32(header->nSampleRate);
}
void DataManager::GetSurface( int ztime)
{
Surface sf;
sf.z = ztime;
long trace_size = (long)(240L+segyHead->DataType*segyHead->hns);
int datatype = segyHead->DataType;
long fp = 0L;
for( int i = 0; i < segyHead->TotalTraces; i += 10 )
{
fp = ( 3600L + (long)i*trace_size);
fseek(filesegy,fp,SEEK_SET);
fread((void*)(&sf.x+8L),sizeof(int),1,filesegy);
sf.x = swap_int32(sf.x);
fseek(filesegy,fp+20L,SEEK_SET);
fread((void*)(&sf.y),sizeof(int),1,filesegy);
sf.y = swap_int32(sf.y);
fseek(filesegy,fp+datatype*sf.z+240L,SEEK_SET);
if( segyHead->format == 1 )
{
fread((void*)(&sf.value),sizeof(float),1,filesegy);
sf.value = ibm2float(sf.value);
}
else if( segyHead->format == 2)
{
int temp = 0;
fread((void*)(&temp),sizeof(int),1,filesegy);
sf.value = swap_int32(temp);
}
else if( segyHead->format == 3 )
{
short temp = 0;
fread((void*)(&temp),sizeof(short),1,filesegy);
sf.value = swap_int16(temp);
}
else if( segyHead->format == 5 )
{
fread((void*)(&sf.value),sizeof(float),1,filesegy);
}
else if( segyHead->format == 8 )
{
char temp = 0;
fread((void*)(&temp),sizeof(char),1,filesegy);
sf.value = (float)temp;
}
surface.push_back(sf);
}
qDebug()<<"Read surface have done!"<<surface.size();
}
static void swap_snoop_packet(struct snoop_packet *p)
{
swap_uint32(&p->caplen);
swap_uint32(&p->len);
swap_uint32(&p->offset);
swap_uint32(&((p->ts).tv_sec));
swap_int32(&((p->ts).tv_usec));
}
void swap_ape_descriptor(APE_DESCRIPTOR *des)
{
des->nVersion = swap_int16(des->nVersion);
des->nDescriptorBytes = swap_int32(des->nDescriptorBytes);
des->nHeaderBytes = swap_int32(des->nHeaderBytes);
des->nSeekTableBytes = swap_int32(des->nSeekTableBytes);
des->nHeaderDataBytes = swap_int32(des->nHeaderDataBytes);
des->nAPEFrameDataBytes = swap_int32(des->nAPEFrameDataBytes);
des->nAPEFrameDataBytesHigh = swap_int32(des->nAPEFrameDataBytesHigh);
des->nTerminatingDataBytes = swap_int32(des->nTerminatingDataBytes);
}
// Process bytes available from the stream
//
// The stream is assumed to contain only our custom message. If it
// contains other messages, and those messages contain the preamble bytes,
// it is possible for this code to become de-synchronised. Without
// buffering the entire message and re-processing it from the top,
// this is unavoidable.
//
// The lack of a standard header length field makes it impossible to skip
// unrecognised messages.
//
bool
AP_GPS_MTK::read(void)
{
uint8_t data;
int16_t numc;
bool parsed = false;
numc = port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = port->read();
restart:
switch(_step) {
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (MESSAGE_CLASS == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
case 3:
if (MESSAGE_ID == data) {
_step++;
_ck_b += (_ck_a += data);
_payload_counter = 0;
} else {
_step = 0;
goto restart;
}
break;
// Receive message data
//
case 4:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Checksum and message processing
//
case 5:
_step++;
if (_ck_a != data) {
_step = 0;
}
break;
case 6:
_step = 0;
if (_ck_b != data) {
break;
}
// set fix type
if (_buffer.msg.fix_type == FIX_3D) {
state.status = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.msg.fix_type == FIX_2D) {
state.status = AP_GPS::GPS_OK_FIX_2D;
}else{
state.status = AP_GPS::NO_FIX;
}
state.location.lat = swap_int32(_buffer.msg.latitude) * 10;
state.location.lng = swap_int32(_buffer.msg.longitude) * 10;
state.location.alt = swap_int32(_buffer.msg.altitude);
state.ground_speed = swap_int32(_buffer.msg.ground_speed) * 0.01f;
state.ground_course_cd = swap_int32(_buffer.msg.ground_course) / 10000;
state.num_sats = _buffer.msg.satellites;
if (state.status >= AP_GPS::GPS_OK_FIX_2D) {
make_gps_time(0, swap_int32(_buffer.msg.utc_time)*10);
}
// we don't change _last_gps_time as we don't know the
// full date
fill_3d_velocity();
parsed = true;
}
}
return parsed;
}
