数据采集代码

源码的数据采集程序，可见第38行其中使用了pollData和update进行数据采集。

void LpmsSensorManager::run(void){    MicroMeasure mm;    float prevTimestamp = 0.0f;    int deviceType = 0;#ifdef _WIN32	    ce.connect();    // be.connect();#endif	#ifdef ANDROID    LOGV("[LpmsSensorManager] Thread running\n");#endif    mm.reset();    int sleepFlag = 0;    while (stopped == false) {        switch (managerState) {        case SMANAGER_MEASURE:            lm.lock();            for (auto i = sensorList.begin(); i != sensorList.end(); i++) {                (*i)->pollData();//数据采集            }#ifdef _WIN32            ce.poll();#endif            lm.unlock();            if (mm.measure() > threadDelay) {                mm.reset();                lm.lock();                for (auto i = sensorList.begin(); i != sensorList.end(); i++) {                    (*i)->update(); //数据采集                }                lm.unlock();            } else {                std::this_thread::sleep_for(std::chrono::microseconds(100));            }            break;        case SMANAGER_LIST:            deviceList.clear();#ifdef _WIN32            ce.listDevices(&deviceList);            // be.listDevices(&deviceList);			#endif            if (managerState != SMANAGER_LIST)                break;            if (scan_serial_ports_ == true)            {                if (verbose)                    logd(TAG, "List RS2323 devices\n");                LpmsRS232::listDevices(&deviceList);            }            // if (managerState != SMANAGER_LIST)                // break;			// LpmsTcp::listDevices(&deviceList);#ifdef BUILD_LPMS_U            if (managerState != SMANAGER_LIST)                break;            LpmsU::listDevices(&deviceList);#endif#ifdef _WIN32            if (managerState != SMANAGER_LIST)                break;            LpmsU2::listDevices(&deviceList);#endif#ifdef BUILD_BLUETOOTH            if (managerState != SMANAGER_LIST)                break;            LpmsBBluetooth::listDevices(&deviceList);#endif            managerState = SMANAGER_MEASURE;            break;        }#ifdef __GNUC__        std::this_thread::sleep_for(std::chrono::milliseconds(1));#endif    }#ifdef _WIN32		    ce.close();    // be.close();#endif}

在其声明的时候就new了一个线程进行run操作

LpmsSensorManager::LpmsSensorManager(JavaVM *thisVm, jobject bluetoothAdapter) :thisVm(thisVm),bluetoothAdapter(bluetoothAdapter)#endif{    stopped = false;    isRecording = false;    threadDelay = 500;    currentUartBaudrate = SELECT_LPMS_UART_BAUDRATE_115200;    verbose = true;    managerState = SMANAGER_MEASURE;    std::thread t(&LpmsSensorManager::run, this); //新建线程，执行run函数#ifdef _WIN32	#ifdef THREAD_HIGH_PRIORITY    HANDLE th = t.native_handle();    SetThreadPriority(th, THREAD_PRIORITY_HIGHEST);#endif#endif    t.detach();#ifdef ANDROID    LOGV("[LpmsSensorManager] Started");#endif}

可见就是不断执行update进行数据的采集，update程序如下：

... ... // Main measurement state    case STATE_MEASURE:        assertConnected();        // Start next measurement step only if program is not waiting for data or ACK        if (bt->isWaitForData() == false && bt->isWaitForAck() == false) {            if (bt->getMode() != SELECT_LPMS_MODE_STREAM) {                bt->setStreamMode();                prepareStream = 0;                break;            }        }        // TODO: Insert error handling for sensor.        // if (bt->isError() == true) {        // setSensorStatus(SENSOR_STATUS_ERROR);        // }        if (paused == true) {            break;        }        if (prepareStream < STREAM_N_PREPARE) {            ++prepareStream;            break;        }        // Load current data from hardware and calculate rotation matrix and Euler angle        if (bt->getLatestImuData(&imuData) == false) break; //可见是从imuDataQueue弹出imuData/*bool LpmsIoInterface::getLatestImuData(ImuData *id){    if (imuDataQueue.empty() == true) return false;    *id = imuDataQueue.front();    imuDataQueue.pop();    return true;}*/        frameTime = lpmsTimer.measure() / 1000.0f;        lpmsTimer.reset();        setFps(frameTime);        convertArrayToLpVector4f(imuData.q, &q);        quaternionToMatrix(&q, &m);        convertLpMatrixToArray(&m, imuData.rotationM);        // Add frame number timestamp and IMU ID to current ImuData        ++frameNo;        imuData.frameCount = frameNo;        imuData.openMatId = configData.openMatId;        setConnectionStatus(SENSOR_CONNECTION_CONNECTED);        if (isMagCalibrationEnabled == true) {            setSensorStatus(SENSOR_STATUS_CALIBRATING);        }        else {            if (paused == false) {                setSensorStatus(SENSOR_STATUS_RUNNING);            }            else {                setSensorStatus(SENSOR_STATUS_PAUSED);            }        }        convertArrayToLpVector3f(imuData.aRaw, &aRaw);        convertArrayToLpVector3f(imuData.bRaw, &bRaw);        convertArrayToLpVector3f(imuData.gRaw, &gRaw);        // Corrects magnetometer measurement        if ((bt->getConfigReg() & LPMS_MAG_RAW_OUTPUT_ENABLED) != 0) {            vectSub3x1(&bRaw, &configData.hardIronOffset, &b);            matVectMult3(&configData.softIronMatrix, &b, &b);        }        else {            vectZero3x1(&b);        }        // Corrects accelerometer measurement        if ((bt->getConfigReg() & LPMS_ACC_RAW_OUTPUT_ENABLED) != 0) {            matVectMult3(&configData.misalignMatrix, &aRaw, &a);            vectAdd3x1(&configData.accBias, &a, &a);        }        else {            vectZero3x1(&a);        }        // Corrects gyro measurement        if ((bt->getConfigReg() & LPMS_GYR_RAW_OUTPUT_ENABLED) != 0) {            matVectMult3(&configData.gyrMisalignMatrix, &gRaw, &g);            vectAdd3x1(&configData.gyrAlignmentBias, &g, &g);        }        else {            vectZero3x1(&g);        }        convertLpVector3fToArray(&a, imuData.a);        convertLpVector3fToArray(&b, imuData.b);        convertLpVector3fToArray(&g, imuData.g);        // Checks, if calibration is active        checkMagCal(frameTime);        checkPlanarMagCal(frameTime);        checkMisalignCal(frameTime);        checkGyrMisalignCal(frameTime);        checkMagMisalignCal(frameTime);        checkMagReferenceCal(frameTime);                // Sets current datac        setCurrentData(imuData); //可见其实现是将数据压到dataQueue,当其长度小于dataQueueLength时。/*void LpmsSensor::setCurrentData(ImuData d){    std::unique_lock
    
      lock(sensorMutex);    currentData = d;    if (dataQueue.size() < dataQueueLength) {        dataQueue.push(d);    }    else {        dataQueue.pop();        dataQueue.push(d);    }    if (lpmsCallback) {        lpmsCallback(d, deviceId.c_str());    }    newDataCondition.notify_one();}*/        // Checks, if data saving is active        checkSaveData(); //检测save与否，并执行操作        break;... ...
    

该程序中值得注意的有两个函数，一个函数是getLatestImuData 可见是从imuDataQueue弹出imuData。

bool LpmsIoInterface::getLatestImuData(ImuData *id){    if (imuDataQueue.empty() == true) return false;    *id = imuDataQueue.front();    imuDataQueue.pop();    return true;}

一个函数是setCurrentData，可见其实现是将数据压到dataQueue,当其长度小于dataQueueLength时。

void LpmsSensor::setCurrentData(ImuData d){    std::unique_lock
    
      lock(sensorMutex);    currentData = d;    if (dataQueue.size() < dataQueueLength) {        dataQueue.push(d);    }    else {        dataQueue.pop();        dataQueue.push(d);    }    if (lpmsCallback) {        lpmsCallback(d, deviceId.c_str());    }    newDataCondition.notify_one();}
    

然后查看我们使用的getCurrentData函数，其是从dataQueue弹出的数据，也就是说不需要跟传感器通信，我们只需要从dataQueue中获取数据即可，但是应该保证数据采集程序在数据采集周期将数据取出，如果不行的话，则会导致数据丢失，即自编上位机时不需要多线程进行数据采集，只使用while循环就可以完成数据采集，多线程反而导致电脑性能不足而导致数据丢失。

ImuData LpmsSensor::getCurrentData(void){    ImuData d;    bt->zeroImuData(&d);    sensorMutex.lock();    if (dataQueue.size() > 0) {        d = dataQueue.front();        dataQueue.pop();    }    else {        d = currentData;    }    sensorMutex.unlock();    return d;}

对于imuDataQueue的获得是在蓝牙程序parseSensorData中实现的。

bool LpmsBle::parseSensorData(void){	unsigned o=0;	const float r2d = 57.2958f;	int iTimestamp;	int iQuat;	int iHeave;	zeroImuData(&imuData); 		fromBufferInt16(oneTx, o, &iTimestamp);	o = o + 2;	currentTimestamp = (float) iTimestamp;		if (timestampOffset > currentTimestamp) timestampOffset = currentTimestamp;	imuData.timeStamp = currentTimestamp - timestampOffset;		fromBufferInt16(oneTx, o, &iQuat);	o = o + 2;	imuData.q[0] = (float) iQuat / (float) 0x7fff;		fromBufferInt16(oneTx, o, &iQuat);	o = o + 2;	imuData.q[1] = (float) iQuat / (float) 0x7fff;	fromBufferInt16(oneTx, o, &iQuat);	o = o + 2;	imuData.q[2] = (float) iQuat / (float) 0x7fff;	fromBufferInt16(oneTx, o, &iQuat);	o = o + 2;	imuData.q[3] = (float) iQuat / (float) 0x7fff;		fromBufferInt16(oneTx, o, &iHeave);	o = o + 2;	imuData.hm.yHeave = (float) iHeave / (float) 0x0fff;		if (imuDataQueue.size() < 64) {		imuDataQueue.push(imuData);	}	return true;}

其中parseSensorData在parseFunction中调用。

bool LpmsIoInterface::parseFunction(void){     ... ...	case GET_SENSOR_DATA:        parseSensorData();        break;     ... ...}

parseFunction在函数parseModbusByte中调用。

bool LpmsBBluetooth::parseModbusByte(void){     ... ...		case PACKET_LRC_CHECK1:            lrcReceived = lrcReceived + ((unsigned)b * 256);            if (lrcReceived == lrcCheck) {                parseFunction();            }            else {                if (verbose) logd(TAG, "Checksum fail in data packet\n");            }            rxState = PACKET_END;            break;     ... ...}

parseModbusByte在checkState中调用。

bool LpemgIoInterface::checkState(void){	parseModbusByte();    ... ...}

checkState在pollData中调用。

void LpmsSensor::pollData(void){    if (bt->deviceStarted() == true) {        if (!bt->pollData())            if (verbose) logd(TAG, "Poll Data error: %s\n", bt->getErrorMsg().c_str());        bt->checkState();    }}

可见pollData实现了从传感器获取数据，保存至imuDataQueue，而update实现了数据处理并将数据保存至dataQueue。

下面是数据采集的子线程。

bool IMUDAQ_Task::IMUDAQ(){	bool first = true;	timeb start, end;	int i[4] = { 0,0,0,0};	int j = 0;	while (1) {		ftime(&start);		ftime(&end);		while ((end.millitm - start.millitm + 1000 * (end.time - start.time) <= period * 1000 || stopbyuser || onceonly)			&& running)		{			j = 0;			for (auto lpms : Lpms) {				if (lpms->hasImuData() > 0) {					imudata = lpms->getCurrentData();					memcpy(Quaternion.data, imudata.q, 4 * sizeof(float));					scalarVectMult4x1(vect4x1Norm(Quaternion), &Quaternion, &QuaternionNormal);					memcpy(LinAcc.data, imudata.linAcc, 3 * sizeof(float));					quatRotVec(QuaternionNormal, LinAcc, &GlobalLinAcc);					if (!send) {						leg = Leg[j / 2];						legposition = Legposition[j % 2];					}					else {						signal.set_leg(ImuTutorial::Signal::Leg(j / 2));						signal.set_legposition(ImuTutorial::Signal::LegPosition(j % 2));					}					savedata();					i[j]++;				}				j++;			}			if (first)			{				first = false;				if (onceonly)					break;			}			ftime(&end);		}		if(end.millitm - start.millitm + 1000 * (end.time - start.time) > period*1000 || stopbyuser || onceonly)			processing = false;		if ((running == false)&&!first)		{			break;		}		if ((running == true) && ((processing == false)|| onceonly)) {			running = false;			break;		}	}	std::cout << std::endl;	for (int n = 0; n < 4; n++)		std::cout << Leg[n / 2] << " " << Legposition[n % 2] << " data lenght：" << i[n] << std::endl;	if (processing == false && !send) IMU_log.close();	t = nullptr;	for (auto lpms:Lpms)		lpms->pause();	std::cout << "Data Acquisition over ！" << std::endl;	std::cout << "Please enter your command : ";	return(true);}

graph TB subgraph lpms pollData-->checkState checkState-->parseModbusByte parseModbusByte-->parseFunction parseFunction-->parseSensorData parseSensorData-->imuDataQueueLength(if imuDataQueueLength > 64) imuDataQueueLength-->|yes push| imuDataQueue imuDataQueueLength-->|no| update imuDataQueue-->update update-->getLatestImuData getLatestImuData-->|pop| imuData imuData-->setCurrentData setCurrentData-->dataQueueLength(if dataQueueLength > 64) dataQueueLength-->|yes push| dataQueue dataQueueLength-->|no| pollData dataQueue-->pollData end subgraph ImuCpp dataQueue-->|pop| getCurrentData getCurrentData-->|push| ImuSendimudataQueue ImuSendimudataQueue-->ImuSendimudataQueueEmpty(if all ImuSendimudataQueue is not empty) ImuSendimudataQueueEmpty-->|yes| PopImudata ImuSendimudataQueueEmpty-->|no|getCurrentData PopImudata-->|!send| ImuSave ImuSave-->getCurrentData PopImudata-->|send| ImuSend ImuSend-->getCurrentData end

PPT演示调用

graph TD subgraph lpms parseSensorData-->imuDataQueue imuDataQueue-->getLatestImuData getLatestImuData-->|pop| setCurrentData setCurrentData-->|push| dataQueue dataQueue-->parseSensorData end subgraph ImuCpp dataQueue-->|pop| getCurrentData getCurrentData-->|push| imudataQueueCpp imudataQueueCpp-->imudataQueueCppEmpty(if all imudataQueueCpp is not empty) imudataQueueCppEmpty-->|yes| PopImudataVector imudataQueueCppEmpty-->|no| getCurrentData PopImudataVector-->|!send| ImuSave ImuSave-->getCurrentData PopImudataVector-->|send| ImuSend ImuSend-->getCurrentData end

Tcp通讯时序详解

首先客户端主动发起连接、发送请求，然后服务器端响应请求，然后客户端主动关闭连接。两条竖线表示通讯的两端，从上到下表示时间的先后顺序，注意，数据从一端传到网络的另一端也需要时间，所以图中的箭头都是斜的。双方发送的段按时间顺序编号为1-10，各段中的主要信息在箭头上标出，例如段2的箭头上标着SYN, 8000(0), ACK1001, ，表示该段中的SYN位置1，32位序号是8000，该段不携带有效载荷（数据字节数为0），ACK位置1，32位确认序号是1001，带有一个mss（Maximum Segment Size，最大报文长度）选项值为1024。

建立连接（三次握手）的过程：

1. 客户端发送一个带SYN标志的TCP报文到服务器。这是三次握手过程中的段1。

客户端发出段1，SYN位表示连接请求。序号是1000，这个序号在网络通讯中用作临时的地址，每发一个数据字节，这个序号要加1，这样在接收端可以根据序号排出数据包的正确顺序，也可以发现丢包的情况，另外，规定SYN位和FIN位也要占一个序号，这次虽然没发数据，但是由于发了SYN位，因此下次再发送应该用序号1001。mss表示最大段尺寸，如果一个段太大，封装成帧后超过了链路层的最大帧长度，就必须在IP层分片，为了避免这种情况，客户端声明自己的最大段尺寸，建议服务器端发来的段不要超过这个长度。

2. 服务器端回应客户端，是三次握手中的第2个报文段，同时带ACK标志和SYN标志。它表示对刚才客户端SYN的回应；同时又发送SYN给客户端，询问客户端是否准备好进行数据通讯。

服务器发出段2，也带有SYN位，同时置ACK位表示确认，确认序号是1001，表示“我接收到序号1000及其以前所有的段，请你下次发送序号为1001的段”，也就是应答了客户端的连接请求，同时也给客户端发出一个连接请求，同时声明最大尺寸为1024。

3. 客户必须再次回应服务器端一个ACK报文，这是报文段3。

客户端发出段3，对服务器的连接请求进行应答，确认序号是8001。在这个过程中，客户端和服务器分别给对方发了连接请求，也应答了对方的连接请求，其中服务器的请求和应答在一个段中发出，因此一共有三个段用于建立连接，称为“三方握手（three-way-handshake）”。在建立连接的同时，双方协商了一些信息，例如双方发送序号的初始值、最大段尺寸等。

在TCP通讯中，如果一方收到另一方发来的段，读出其中的目的端口号，发现本机并没有任何进程使用这个端口，就会应答一个包含RST位的段给另一方。例如，服务器并没有任何进程使用8080端口，我们却用telnet客户端去连接它，服务器收到客户端发来的SYN段就会应答一个RST段，客户端的telnet程序收到RST段后报告错误Connection refused：

$ telnet 192.168.0.200 8080

Trying 192.168.0.200...

telnet: Unable to connect to remote host: Connection refused

数据传输的过程：

1. 客户端发出段4，包含从序号1001开始的20个字节数据。

2. 服务器发出段5，确认序号为1021，对序号为1001-1020的数据表示确认收到，同时请求发送序号1021开始的数据，服务器在应答的同时也向客户端发送从序号8001开始的10个字节数据，这称为piggyback。

3. 客户端发出段6，对服务器发来的序号为8001-8010的数据表示确认收到，请求发送序号8011开始的数据。

在数据传输过程中，ACK和确认序号是非常重要的，应用程序交给TCP协议发送的数据会暂存在TCP层的发送缓冲区中，发出数据包给对方之后，只有收到对方应答的ACK段才知道该数据包确实发到了对方，可以从发送缓冲区中释放掉了，如果因为网络故障丢失了数据包或者丢失了对方发回的ACK段，经过等待超时后TCP协议自动将发送缓冲区中的数据包重发。

关闭连接（四次握手）的过程：

由于TCP连接是全双工的，因此每个方向都必须单独进行关闭。这原则是当一方完成它的数据发送任务后就能发送一个FIN来终止这个方向的连接。收到一个FIN只意味着这一方向上没有数据流动，一个TCP连接在收到一个FIN后仍能发送数据。首先进行关闭的一方将执行主动关闭，而另一方执行被动关闭。

1. 客户端发出段7，FIN位表示关闭连接的请求。

2. 服务器发出段8，应答客户端的关闭连接请求。

3. 服务器发出段9，其中也包含FIN位，向客户端发送关闭连接请求。

4. 客户端发出段10，应答服务器的关闭连接请求。

建立连接的过程进行三次握手连接，而关闭连接通常需要4个段，服务器的应答和关闭连接请求通常不合并在一个段中，因为有连接半关闭的情况，这种情况下客户端关闭连接之后就不能再发送数据给服务器了，但是服务器还可以发送数据给客户端，直到服务器也关闭连接为止。所以由于Sever端发送确认信息和数据信息只用了一次握手，所以比断开少一次，TCP连接是全双工的，因此每个方向都必须单独进行关闭，所以进行了四次握手断开

根据上述所讲写出Cpp与Python程序的通讯的握手时序图

sequenceDiagram participant PythonClient participant CppSever opt connect note over PythonClient,CppSever:三次握手连接确定双方信息 PythonClient->>CppSever:SYN,1000(0),<mss 1460> CppSever->>PythonClient:SYN,8000(0),ACK 1001,<mss 1024> PythonClient->>CppSever:ACK 8001 end loop transfer note over PythonClient,CppSever:Cpp一直向Python单向传输数据 CppSever->>PythonClient:8001(258),ACK 1001 PythonClient->>CppSever:ACK 8259 end opt disconnect note over PythonClient,CppSever:四次握手断开，每个方向单独断开 PythonClient->>CppSever:FIN,1021(0),ACK 8001 CppSever->>PythonClient:ACK 1022 CppSever->>PythonClient:FIN,8001(0),ACK 1022 PythonClient->>CppSever:ACK 8002 end