LPMS-B2 数据采集源码分析

本文是针对 LPMS-B2 数据采集源码进行分析， LPMS-B2 是 LP公司出品的一款IMU，最近用到特别总结。

源码的数据采集程序，可见第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
");
#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
");
                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<std::mutex> 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<std::mutex> 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
");
            }

            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
", 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 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 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