From e96953b54a2174ffcf1b8cfa0da557985e60fa43 Mon Sep 17 00:00:00 2001 From: tangchao0503 <735056338@qq.com> Date: Sat, 24 Dec 2022 16:59:41 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=EF=BC=9A=201.=20=E9=80=9A?= =?UTF-8?q?=E8=BF=87=E5=86=85=E5=AD=98=E6=B1=A0=E8=A7=A3=E5=86=B3=E4=B8=A2?= =?UTF-8?q?=E5=B8=A7=E9=97=AE=E9=A2=98=EF=BC=9B=202.=20=E8=A7=A3=E6=9E=90?= =?UTF-8?q?=E6=83=AF=E5=AF=BC=E5=8D=AB=E6=98=9F=E4=B8=AA=E6=95=B0=EF=BC=8C?= =?UTF-8?q?=E5=B9=B6=E9=80=9A=E8=BF=87socket=E5=8F=91=E9=80=81=E7=BB=99psd?= =?UTF-8?q?k=EF=BC=9B=203.=20=E5=8F=96=E6=B6=88=E9=80=9A=E8=BF=87sbg?= =?UTF-8?q?=E5=8D=AB=E6=98=9F=E6=97=B6=E9=97=B4=E8=AE=BE=E7=BD=AElinux?= =?UTF-8?q?=E7=B3=BB=E7=BB=9F=E6=97=B6=E9=97=B4=EF=BC=8C=E6=AD=A4=E5=8A=9F?= =?UTF-8?q?=E8=83=BD=E4=BA=A4=E7=BB=99psdk=E5=81=9A=EF=BC=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 4 +- Header_Files/MemoryPool.h | 98 +++++++++++++++ Header_Files/MemoryPool.tcc | 235 +++++++++++++++++++++++++++++++++++ Header_Files/sbgrecorder.h | 3 +- Header_Files/udpserver.h | 2 +- Header_Files/ximeaimager.h | 55 ++++++++ Source_Files/sbgrecorder.cpp | 16 +-- Source_Files/udpserver.cpp | 8 +- Source_Files/ximeaimager.cpp | 131 ++++++++++++++----- 9 files changed, 506 insertions(+), 46 deletions(-) create mode 100644 Header_Files/MemoryPool.h create mode 100644 Header_Files/MemoryPool.tcc diff --git a/CMakeLists.txt b/CMakeLists.txt index 6a30c46..65d062c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,7 +41,9 @@ add_executable(${CMAKE_PROJECT_NAME} Source_Files/ximeaimager.cpp Header_Files/ximeaimager.h Source_Files/configfile.cpp - Header_Files/configfile.h) + Header_Files/configfile.h + Header_Files/MemoryPool.tcc + Header_Files/MemoryPool.h) qt5_use_modules(${CMAKE_PROJECT_NAME} ${QT}) target_link_libraries(${CMAKE_PROJECT_NAME} diff --git a/Header_Files/MemoryPool.h b/Header_Files/MemoryPool.h new file mode 100644 index 0000000..567c28f --- /dev/null +++ b/Header_Files/MemoryPool.h @@ -0,0 +1,98 @@ +/*- + * Copyright (c) 2013 Cosku Acay, http://www.coskuacay.com + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * IN THE SOFTWARE. + */ + +#ifndef MEMORY_POOL_H +#define MEMORY_POOL_H + +#include +#include + +template +class MemoryPool +{ + public: + /* Member types */ + typedef T value_type; + typedef T* pointer; + typedef T& reference; + typedef const T* const_pointer; + typedef const T& const_reference; + typedef size_t size_type; + typedef ptrdiff_t difference_type; + typedef std::false_type propagate_on_container_copy_assignment; + typedef std::true_type propagate_on_container_move_assignment; + typedef std::true_type propagate_on_container_swap; + + template struct rebind { + typedef MemoryPool other; + }; + + /* Member functions */ + MemoryPool() noexcept; + MemoryPool(const MemoryPool& memoryPool) noexcept; + MemoryPool(MemoryPool&& memoryPool) noexcept; + template MemoryPool(const MemoryPool& memoryPool) noexcept; + + ~MemoryPool() noexcept; + + MemoryPool& operator=(const MemoryPool& memoryPool) = delete; + MemoryPool& operator=(MemoryPool&& memoryPool) noexcept; + + pointer address(reference x) const noexcept; + const_pointer address(const_reference x) const noexcept; + + // Can only allocate one object at a time. n and hint are ignored + pointer allocate(size_type n = 1, const_pointer hint = 0); + void deallocate(pointer p, size_type n = 1); + + size_type max_size() const noexcept; + + template void construct(U* p, Args&&... args); + template void destroy(U* p); + + template pointer newElement(Args&&... args); + void deleteElement(pointer p); + + private: + union Slot_ { + value_type element; + Slot_* next; + }; + + typedef char* data_pointer_; + typedef Slot_ slot_type_; + typedef Slot_* slot_pointer_; + + slot_pointer_ currentBlock_; + slot_pointer_ currentSlot_; + slot_pointer_ lastSlot_; + slot_pointer_ freeSlots_; + + size_type padPointer(data_pointer_ p, size_type align) const noexcept; + void allocateBlock(); + + static_assert(BlockSize >= 2 * sizeof(slot_type_), "BlockSize too small."); +}; + +#include "MemoryPool.tcc" + +#endif // MEMORY_POOL_H diff --git a/Header_Files/MemoryPool.tcc b/Header_Files/MemoryPool.tcc new file mode 100644 index 0000000..562d958 --- /dev/null +++ b/Header_Files/MemoryPool.tcc @@ -0,0 +1,235 @@ +/*- + * Copyright (c) 2013 Cosku Acay, http://www.coskuacay.com + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * IN THE SOFTWARE. + */ + +#ifndef MEMORY_BLOCK_TCC +#define MEMORY_BLOCK_TCC + + + +template +inline typename MemoryPool::size_type +MemoryPool::padPointer(data_pointer_ p, size_type align) +const noexcept +{ + uintptr_t result = reinterpret_cast(p); + return ((align - result) % align); +} + + + +template +MemoryPool::MemoryPool() +noexcept +{ + currentBlock_ = nullptr; + currentSlot_ = nullptr; + lastSlot_ = nullptr; + freeSlots_ = nullptr; +} + + + +template +MemoryPool::MemoryPool(const MemoryPool& memoryPool) +noexcept : +MemoryPool() +{} + + + +template +MemoryPool::MemoryPool(MemoryPool&& memoryPool) +noexcept +{ + currentBlock_ = memoryPool.currentBlock_; + memoryPool.currentBlock_ = nullptr; + currentSlot_ = memoryPool.currentSlot_; + lastSlot_ = memoryPool.lastSlot_; + freeSlots_ = memoryPool.freeSlots; +} + + +template +template +MemoryPool::MemoryPool(const MemoryPool& memoryPool) +noexcept : +MemoryPool() +{} + + + +template +MemoryPool& +MemoryPool::operator=(MemoryPool&& memoryPool) +noexcept +{ + if (this != &memoryPool) + { + std::swap(currentBlock_, memoryPool.currentBlock_); + currentSlot_ = memoryPool.currentSlot_; + lastSlot_ = memoryPool.lastSlot_; + freeSlots_ = memoryPool.freeSlots; + } + return *this; +} + + + +template +MemoryPool::~MemoryPool() +noexcept +{ + slot_pointer_ curr = currentBlock_; + while (curr != nullptr) { + slot_pointer_ prev = curr->next; + operator delete(reinterpret_cast(curr)); + curr = prev; + } +} + + + +template +inline typename MemoryPool::pointer +MemoryPool::address(reference x) +const noexcept +{ + return &x; +} + + + +template +inline typename MemoryPool::const_pointer +MemoryPool::address(const_reference x) +const noexcept +{ + return &x; +} + + + +template +void +MemoryPool::allocateBlock() +{ + // Allocate space for the new block and store a pointer to the previous one + data_pointer_ newBlock = reinterpret_cast + (operator new(BlockSize)); + reinterpret_cast(newBlock)->next = currentBlock_; + currentBlock_ = reinterpret_cast(newBlock); + // Pad block body to staisfy the alignment requirements for elements + data_pointer_ body = newBlock + sizeof(slot_pointer_); + size_type bodyPadding = padPointer(body, alignof(slot_type_)); + currentSlot_ = reinterpret_cast(body + bodyPadding); + lastSlot_ = reinterpret_cast + (newBlock + BlockSize - sizeof(slot_type_) + 1); +} + + + +template +inline typename MemoryPool::pointer +MemoryPool::allocate(size_type n, const_pointer hint) +{ + if (freeSlots_ != nullptr) { + pointer result = reinterpret_cast(freeSlots_); + freeSlots_ = freeSlots_->next; + return result; + } + else { + if (currentSlot_ >= lastSlot_) + allocateBlock(); + return reinterpret_cast(currentSlot_++); + } +} + + + +template +inline void +MemoryPool::deallocate(pointer p, size_type n) +{ + if (p != nullptr) { + reinterpret_cast(p)->next = freeSlots_; + freeSlots_ = reinterpret_cast(p); + } +} + + + +template +inline typename MemoryPool::size_type +MemoryPool::max_size() +const noexcept +{ + size_type maxBlocks = -1 / BlockSize; + return (BlockSize - sizeof(data_pointer_)) / sizeof(slot_type_) * maxBlocks; +} + + + +template +template +inline void +MemoryPool::construct(U* p, Args&&... args) +{ + new (p) U (std::forward(args)...); +} + + + +template +template +inline void +MemoryPool::destroy(U* p) +{ + p->~U(); +} + + + +template +template +inline typename MemoryPool::pointer +MemoryPool::newElement(Args&&... args) +{ + pointer result = allocate(); + construct(result, std::forward(args)...); + return result; +} + + + +template +inline void +MemoryPool::deleteElement(pointer p) +{ + if (p != nullptr) { + p->~value_type(); + deallocate(p); + } +} + + + +#endif // MEMORY_BLOCK_TCC diff --git a/Header_Files/sbgrecorder.h b/Header_Files/sbgrecorder.h index b20274a..1109af4 100644 --- a/Header_Files/sbgrecorder.h +++ b/Header_Files/sbgrecorder.h @@ -69,8 +69,7 @@ namespace sbgtc void sbgSolutionModeSignal(int); - void sbgAccuracySignal(int); - + void sbgAccuracySignal(int, int); }; } diff --git a/Header_Files/udpserver.h b/Header_Files/udpserver.h index c0c74c3..4b1f8d2 100644 --- a/Header_Files/udpserver.h +++ b/Header_Files/udpserver.h @@ -63,7 +63,7 @@ public slots: void sendSerialPortStatus(int serialPortStatus); void sendSbgSolutionModeState(int SolutionMode); - void sendSbgAccuracyState(int Accuracy); + void sendSbgAccuracyState(int Accuracy,int SatelliteCounter); void sendXimeaImageStatus(int ximeaImageStatus); void sendCopyFileStatus(int fileStatus); diff --git a/Header_Files/ximeaimager.h b/Header_Files/ximeaimager.h index c35c623..4df67f0 100644 --- a/Header_Files/ximeaimager.h +++ b/Header_Files/ximeaimager.h @@ -37,6 +37,11 @@ #include "math.h" #include "utility_tc.h" +#include "MemoryPool.h" +#include +#include + + //#ifdef WIN32 //#include // Windows @@ -44,6 +49,23 @@ //#include // Linux, OSX //#endif +//struct DataBuffer +//{ +// unsigned short data[409200/2]; +//}; + +class DataBuffer +{ +public: + DataBuffer(); + ~DataBuffer(); + + unsigned short data[425600];//304*1400=425600，为了兼容所有设置spectral bin和spatial bin + +private: + +}; + class RecordXimeaTemperature : public QObject { Q_OBJECT @@ -64,6 +86,30 @@ signals: }; +//queue q; +//static QMutex r_qtx{ QMutex::Recursive }; +static std::mutex r_qtx; + +class WriteData2Disk : public QObject +{ +Q_OBJECT + +public: + WriteData2Disk(); + void setParm(queue * q, QString baseFileName, int frameSizeInByte, MemoryPool * pool); + +private: + queue * m_q; + QString m_QbaseFileName; + int m_iFrameSizeInByte; + MemoryPool * m_pool; + +public slots: + void write2Disk(); + +signals: +}; + class XimeaImager : public QObject { Q_OBJECT @@ -99,11 +145,17 @@ private: QThread * m_recordTempThread; RecordXimeaTemperature * m_ximeaTemperature; + QThread * writeData2DiskThread; + WriteData2Disk * writeData2Disk; + queue * q; + MemoryPool * m_pool; + QString m_baseFileName; QString m_ximeaTemperatureCSVPath; Iris::IrisXimeaImager m_imager; unsigned short * m_buffer; +// MemoryPool m_pool; bool m_bRecordControl; int m_iFrameCounter; int m_iFrameSizeInByte; @@ -132,6 +184,8 @@ private: Configfile m_configfile; + + public slots: void openImger(); void closeImger(); @@ -143,5 +197,6 @@ signals: void ximeaImageStatus(int); void recordXimeaTemperatureSignal(QString); + void startWriteDiskSignal(); }; #endif // XIMEAIMAGER_H diff --git a/Source_Files/sbgrecorder.cpp b/Source_Files/sbgrecorder.cpp index 2e43f12..c74e3fb 100644 --- a/Source_Files/sbgrecorder.cpp +++ b/Source_Files/sbgrecorder.cpp @@ -671,13 +671,11 @@ void sbgtc::SbgRecorder::parseSbgMessage(QByteArray * sbgMessage) } else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_GPS1_POS) { -// std::cout<<"纬度精度为："<(maximal)); +// std::cout<<"纬度精度为："<(maximal), satelliteCounter); if(maximal<7) { @@ -710,15 +711,10 @@ void sbgtc::SbgRecorder::parseSbgMessage(QByteArray * sbgMessage) char setGpsTimeCommand[256]; sprintf(setGpsTimeCommand,"date --set=\"%d%02d%02d %02d:%02d:%02d\"",year,month,day,hour,minute,second);//02中的2代表2位数字，0代表以0补全 - system(setGpsTimeCommand);// +// system(setGpsTimeCommand); m_bIsSyncSystemTimeBaseGpstime=true; } - else if(receivedMsgClass==SBG_ECOM_CLASS_LOG_ECOM_0 && receivedMsg==SBG_ECOM_LOG_EKF_EULER) - { -// std::cout<<"1111111111111"<<"UTC time:"<<(int)logData.utcData.year<<","<<(int)logData.utcData.month<<","<<(int)logData.utcData.day<<","<<(int)logData.utcData.hour<<","<<(int)logData.utcData.minute<<","<<(int)(int)logData.utcData.second<moveToThread(m_RecordThread); - m_RecordThread->start(); + m_RecordThread->start(QThread::HighestPriority); m_CopyFileThread=new QThread(); m_copyFile=new FileOperation(); @@ -47,7 +47,7 @@ UdpServer::UdpServer() connect(m_sbgRecorder, SIGNAL(serialPortStatus(int)),this, SLOT(sendSerialPortStatus(int))); connect(m_sbgRecorder, SIGNAL(sbgSolutionModeSignal(int)),this, SLOT(sendSbgSolutionModeState(int))); - connect(m_sbgRecorder, SIGNAL(sbgAccuracySignal(int)),this, SLOT(sendSbgAccuracyState(int))); + connect(m_sbgRecorder, SIGNAL(sbgAccuracySignal(int,int)),this, SLOT(sendSbgAccuracyState(int,int))); connect(m_imager, SIGNAL(ximeaImageStatus(int)),this, SLOT(sendXimeaImageStatus(int))); connect(m_copyFile, SIGNAL(copyFileStatus(int)),this, SLOT(sendCopyFileStatus(int))); @@ -284,13 +284,13 @@ void UdpServer::sendSbgSolutionModeState(int SolutionMode) m_udpSocket->writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); } -void UdpServer::sendSbgAccuracyState(int Accuracy) +void UdpServer::sendSbgAccuracyState(int Accuracy,int SatelliteCounter) { // std::cout<<"UdpServer::sendSbgAccuracyState---------------------:"<< Accuracy <writeDatagram(datagram2send.data(),datagram2send.size(),m_clientIpAddress, 45455); diff --git a/Source_Files/ximeaimager.cpp b/Source_Files/ximeaimager.cpp index c5852f2..b54e7dd 100644 --- a/Source_Files/ximeaimager.cpp +++ b/Source_Files/ximeaimager.cpp @@ -21,6 +21,15 @@ XimeaImager::XimeaImager() m_recordTempThread->start(); connect(this, SIGNAL(recordXimeaTemperatureSignal(QString)),m_ximeaTemperature, SLOT(recordTemperature(QString))); + + writeData2DiskThread = new QThread(); + writeData2Disk = new WriteData2Disk(); + writeData2Disk->moveToThread(writeData2DiskThread); + writeData2DiskThread->start(QThread::HighestPriority); + connect(this, SIGNAL(startWriteDiskSignal()), writeData2Disk, SLOT(write2Disk())); + + m_pool = new MemoryPool; + q = new queue; } void XimeaImager::openImger() @@ -77,6 +86,7 @@ void XimeaImager::openImger() int frameSizeAuto = m_imager.getBufferSizeOfOneFrame(); m_iFrameSizeInByte = frameSizeAuto; + std::cout<<"每一帧的字节数：-------------------------------"<< m_iFrameSizeInByte <setParm(q,m_baseFileName,m_iFrameSizeInByte, m_pool); + emit startWriteDiskSignal(); + m_imager.start(); struct timeval timeStart, timeEnd; double runTime=0; gettimeofday(&timeStart, NULL); - while (m_bRecordControl) { unsigned short *x=m_imager.get_frame(m_buffer); //fwrite(m_buffer,2,getBandCount()*getSampleCount(),hFile); - fwrite(m_imager.m_image.bp,static_cast(m_imager.m_image.bp_size),1,hFile); +// fwrite(m_imager.m_image.bp,static_cast(m_imager.m_image.bp_size),1,hFile); //fflush(hFile);//只保证了将IO缓冲写入系统缓冲中，使IO读操作能成功，但系统什么时候写入磁盘，由系统决定，一般是达到一定量时系统他就写入磁盘。 //sync();//强制系统将系统文件缓冲的内容写入磁盘 - m_iFrameCounter+=1; -// if(m_iFrameCounter==100) -// break; + + r_qtx.lock(); + DataBuffer * buffer = m_pool->newElement(); + r_qtx.unlock(); +// m_pool->construct(buffer); + +// memcpy(buffer->data,(unsigned short *)m_imager.m_image.bp,m_iFrameSizeInByte/2); + memcpy((void *)buffer->data,m_imager.m_image.bp,m_iFrameSizeInByte); + + r_qtx.lock(); + q->push(buffer); + r_qtx.unlock();// + +// std::cout<<"XimeaImager::startRecord，队列长度为："<< q->size() < 5*60) //这个判断会导致丢帧率的大幅升高：5% → 20%，推测原因：除法耗时； -// { -// break; -// } - // unsigned char pixel = *(unsigned char*)image.bp;//Default value: XI_MONO8 // unsigned short pixel =*(unsigned short*)image.bp;//XI_RAW16 // printf("Image %d (%dx%d) received from camera. First pixel value: %d\n", m_iFrameCounter, (int)image.width, (int)image.height, pixel); } - gettimeofday(&timeEnd, NULL); runTime = (timeEnd.tv_sec - timeStart.tv_sec ) + (double)(timeEnd.tv_usec -timeStart.tv_usec)/1000000; + m_imager.stop(); double frameInTheory=runTime * getFramerate(); double frameLossed=frameInTheory - m_iFrameCounter; @@ -514,12 +525,10 @@ void XimeaImager::startRecord(double TimeDifferenceBetweensOSAndSbg,QString base std::cout<<"丢失帧数为: "<empty(); + r_qtx.unlock(); + if(bempty) + { + QThread::msleep(sleepCounters * sleepCounters * 10); +// std::cout<<"WriteData2Disk::write2Disk-----------------------队列第几次为空：" << sleepCounters <front(); + memcpy(dataBuffer,buffer->data,m_iFrameSizeInByte); +// m_pool->destroy(m_q->front()); + m_pool->deleteElement(buffer); + m_q->pop(); + r_qtx.unlock(); + +// std::cout<<"WriteData2Disk::write2Disk-----------------------正在写磁盘!" << m_pool->max_size() <max_size() < * q, QString baseFileName, int frameSizeInByte, MemoryPool * pool) +{ + m_q = q; + m_QbaseFileName = baseFileName; + m_iFrameSizeInByte = frameSizeInByte; + + m_pool = pool; +}